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ABSTRACT 

The  development  and  testing  of  Autonomous  Underwater  Vehicle  (AUV) 
hardware  and  software  is  greatly  complicated  by  vehicle  inaccessibility  during 
operation.  Integrated  simulatioRiremotely  links  vehicle  components  and  support 
equipment  with  graphics  simulation  workstations,  allowing  complete  real-time, 
pre-mission,  pseudo-mission  and  post-mission  visualization  and  analysis  in  the  lab 
environment.  Integrated  simulator  testing  of  software  and  hardware  is  a  broad  and 
versatile  method  that  supports  rapid  and  robust  diagnosis  and  correction  of  system 
faults.  This  method  is  demonstrated  using  the  Naval  Postgraduate  School 
(NPS)  AUV. 

High-resolution  three-dimensional  graphics  workstations  can  provide  real-time 
representations  of  vehicle  dynamics,  control  system  behavior,  mission  execution, 
sensor  processing  and  object  classification.  Integrated  simulation  is  also  useful  for 
development  of  the  variety  of  sophisticated  artificial  intelligence  applications  needed 
by  an  AUV.  Examples  include  sonar  classification  using  an  expert  system  and  path 
planning  using  a  circle  world  model. 

The  flexibility  and  versatility  provided  by  this  approach  enables  visualization  and 
analysis  of  all  aspects  of  AUV  development.  Integrated  simulator  networking  is 
recommended  as  a  fundamental  requirement  for  , AUV  research  and  deployment. 
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I.  INTRODUCTION 

A,  PROBLEM  STATEMENT 

Designing,  building  and  testing  an  Autonomous  Underwater  Vehicle  (AUV)  is 
difficult.  AUVs.must  operate  unattended  and  uncontrolled  in  a  remote  and 
unforgiving  environment.  Inaccessibility  greatly  complicates  evaluation,  diagnosis  and 
correction  of  AUV  system  faults.  In  order  to  ensure  complete  reliability,  AUV 
software  and  hardware  need  to  be  fully  tested  in  the  laboratory  before  operational 
deployment.  Such  important  testing  requirements  cannot  be  met  using  only  a 
standalone  AUV. 

Designing  an  AUV  is  also  complex.  Many  capabilities  are  required  for  a  mobile 
robot,  to  act  independently.  Sensing,  motion  control,  motion  planning,  mission 
planning,  failure  recovery  and  overall  control  are  all  essential.  Interaction  between 
vehicle  processes  and  the  mechanics  of  actual  implementation  must  also  be  solved. 
These  complex  problems  cannot  be  modeled,  simulated  or  integrated  into  an 
autonomous  mobile  robot  without  understanding  their  fundamental  principles  and 
difficulties. 

The  primary  problem  addressed  by  this  thesis  is  how  to  design  and  construct  an 
integrated  simulator  in  order  to  completely  visualize  AUV  performance  in  support  of 
distributed  research  and  technical  evaluation.  All  aspects  of  AUV  software  design  and 
simulation  are  considered.  As  direct  examples  of  how  integrated  simulation  may  be 
applied,  in-depth  analysis  is  also  provided  for  the  future  roles  of  naval  AUVs,.  sensor 
analysis,  path  planning  and  real-time  interaction. 
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B.  MOTIVATION 

The  principal  motiyation  driving  the  development  of  an  AUV  integrated 
simulator  is  to  meet  the  research  needs  of  the  large  academic  group  working  on  the 
Naval  Postgraduate  School  (NPS)  AUV.  Students  and  professors  alike  have  diverse 
research  goals  that  are  often  forced  to  compete  for  access  to  vehicle  system  software 
and  limited  pool  test  time.  The  need  to  use  operational  software  running  on  actual 
NPS  AUV  hardware  is  a  particularly  important  requirement.  Lack  of  accessibility  to 
the  NPS  AUV  in  a  distributed  laboratory  environment  has  occasionally  prevented 
implementing  new  software  applications  on  the  vehicle.  Pre-mission  validation  of 
vehicle  systems  response  to  new  software  has  been  similarly  limited  in  scope,  resulting, 
in  several  operational  test  failures  and  frustrating  delays  in  development.  Integrated 
simulation  is  a  high-level  tool  that  enables  solutions  to  all  of  these  challenges. 

Scientific  visualization  of  complex  interactions  greatly  improves  our 
understanding  of  how  things  work.  Human  beings  are  visually  oriented.  Being  able 
to  see  and  control  a  moving  picture  allows  us  to  quickly  and  intuitively  understand 
numerous  process  interactions.  A  fundamental  computer  science  tenet  first  expressed 
by  Dr.  Richard  A.  Hamming  is  that  "The  purpose  of  computing  is  insight,  not 
numbers"  (Hamming  73).  Integrated  simulation  is  intended  to  provide  insight. 

The  term  artificial  intelligence  typically  refers  to  the  study  of  how  to  perform 

tasks' that  are  usually  considered  to  require  human  intelligence.  Numerous  such 

artificial  intelligence  tasks  are  required  for  a  mobile  robot  to  achieve  autonomy. 

The  importance  of  solving  artificial  intelligence  problems  is  widely  recognized. 

Prominent  robotics  researcher  Hans  P.  Moravec  states, 

"...solving  the  day  to  day  problems  of  developing  a  mobile  organism  steers  one 
in  the  direction  of  general  intelligence,  while  working  on  the  problems  of  a  fixed 
entity  is  more  likely  to  result  in  very  specialized  solutions...  Mobile  robotics  may 
or  may  not  be  the  fastest  way  to  arrive  at  general  human  competence  in 
machines,  but  I  believe  it  is  one  of  the  surest  roads."  (Moravec  83) 

This  study  of  integrated  simulation  has  helped  reveal  valuable  conclusions  regarding 

the  artificial  intelligence  subjects  of  sensor  analysis  and  path  planning. 
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The  NPS  AUV  Integrated  Simulator  has  been  designed  to  support  complete 
scientific  visualization  of  actual  NPS  AUV  vehicle  .performance.  The  lessons  learned 
while  building  this  integrated  simulator  have  proven  that  disttibuted  research  can  be 
effectively  accomplished  when  proper  network  connections:  and  data-passing 
mechanisms  are  provided.  The  integrated  simulation  approach, has  great  value  and 
general  applicability  for  the  rapid  development  of  all  types  of  mobile  robots. 

C.  OBJECTIVES 

This  thesis  addresses  the  following  research. questions: 


•  How  can  an  integrated  simulator  be  constructed  to  support  pre-mission, 
pseudo-mission  and  post-mission  AUV  evaluation? 

•  How  can  integrated  simulation  support  distributed  research? 

•  What  is  required  to  allow  both  local  and  remote  mission  performance  analysis? 

•  How  can  a  laboratory  AUV  microprocessor  be  used  for  preliminary  AUV  testing 
in  a  distributed  research  environment? 

•  How  can  the  numerous  processes  that  make  up  NPS  AUV  control  software 
communicate  with  the  NPS  AUV  graphics  simulation  program? 

•  How  can  inter-process  communication  be  accomplished  identically  and 
independently  regardless  of  where  AUV  software  is  running? 

•  How  can  the  Computer  Science  Department  Network,  Campus-Wide  Network 
and  NPS  AUV  be  linked  together  to  connect  the  variety  of  processors  and 
operating  systems  that  support  the  NPS  AUV? 

•  How  can  active  sonar  range  and  bearing  data  be  analyzed  to  classify  sonar 
contacts? 

•  How  can  a  shortest  path  be  found  around  circular  or  cylindrical  obstacles? 

•  What  real-time  operating  system  considerations  must  be  met  in  order  to  support 
parallel  operation  by  mutually-cooperating  artificial  intelligence  AUV 
applications  interacting  with  a  real-time  environment  or  a  near-real-time 
simulation? 
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D.  THESIS  ORGANIZATION 

Many  components  and  many  concepts  make  up  an  integrated:  simulator.  Conduct 
of  this  research  led  down  many  interesting  intellectual  trails.  Consequently  the  scope 
of  material  contained  in  this  thesis  is  broad  while  the  many  individual  conclusions  are 
detailed.  Given  this  diversity  of  materiah  the  objectives,  summaries  of  previbusjwork 
and  conclusions  are  included  with  each  pet  tinent  chapter.  When  appropriate,  thesis 
chapters  have  doubled  as  separate.  articles  to  report  on  results  of  general  interest.  The 
five  sections  that  have  also  been- written  for  independent  publication  are  identified 
below,  as  are  the  contributing  coauthors. 

Chapter  II  describes  AUV  research  at  NPS.  It  was  originally  written  as  a  survey 
article  to  summarize  the  riiany  facets  of  AUV  research  at  NPS,  as  well- as  describe 
expected  future  roles  of  naval  AUVs.  This  chapter  is  an  important  prelude  to  the 
thesis  in  that  it  establishes  the  scope  of  current  and  future  AUV  work  that  an 
integrated  simulator  must  support.  This  chapter  appeared  as  an  article  in 
Sea  Technology  and  was  cowritten  with  LCDR  Marl:  A.  Compton  USN' 

(Brutzman  Compton  91). 

Chapter  III  defines  and  develops  the  concept  of  rapid  simulation  for  rapid 
development  of  AUVs.  As  such  it  is  the  heart  of  this  thesis.  This  chapter  is  accepted 
for  presentation  to  the  IEEE  Oceanic  Engineering  Society  Symposium  on  Autonomous 
Underwater  Vehicles  1992.  Dr.  Yuiaka  Kanayama  andDr.  Michael  J.  Zyda  are 
coauthors  of  the  corresponding  article* (Brutzman  Kanayama  Zyda  92). 

Chapter  IV  presents  design  specifications  that  are  pertinent  to  the  specific 
integrated  simulator  implemented  for  the  NPS  AUV.  Chapter  V  describes  specific 
data  network  requirements  for  the  NPS  AUV  Integrated  Simulator.  Details  are 
included  that  are  based  on.  the  difficulties  and  successes  encountered  during 
implementation.  The  technical  skills  and  determined  efforts  of  Mr.  Russell  Whalen 
and  Mr.  Walter  Landaker  were  instrumental  in  building  the  laboratory  AUV  and 
connecting  it  to  the  network. 
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Chapter  VI  presents  fundamental  work  on  the  effective  synthesis  of  geometric 


analysis  and  expert  system  heuristics  for  classifying  underwater  objects.  This  chapter 
was  also  submitted  in  article  form  for  presentation  to  the  IEEE  Oceanib  Engineering 
Society  Conference  OCEANS  92.  This  work  was  cowritten  with  LGDR  Mark  A. 
Compton  USN  and  Dr.  Yutaka  Kanayama  (Brutzman  Compton  Kanayama  92). 
Examples  of  applicability  to  integrated  simulation  are  included. 

Chapter  VII  presents  fundamental  work  on  shortest  path  planning  using  circular 
obstacles.  This  work  is  based  primarily  on  Dr.  Yutaka  Kanayama’s  theories  of 
optimal  robot  motion.  Examples  of  applicability  to  integrated  simulation  are  included 
in  the  chapter. 

Chapter  VIII  describe  real-time  operating  system  considerations,  all  of  which  are 
pertinent  both  to  an  operating  AUV  and  an  AUV  integrated  simulator.  This  chapter 
has  also  been  included  in  a  NPS  technical  report  (Badr  Byrnes  Brutzman  Nelson  92) 
and  was  edited  by  Dr.  Michael  L.  Nelson,  MAJ  USAF. 

Chapter  IX  presents  NPS  AUV  Integrated  Simulator  limitations  and  performance 
measurements.  Chapter  X  discusses  the  many  promising  opportunities  for  potential 
future  research  and  follow-on  work  using  the  NPS  AUV  Integrated  Simulator. 

Chapter  XI  presents  conclusions  and  recommendations. 

Appendix  A  is  a  guide  for  NPS  AUV  users  and  software  developers  who  are 
interested  in  utilizing  the  NPS  AUV  Integrated  Simulator.  Appendix  B  is  a  synopsis 
of  the  lengthy  source  code  written  for  the  graphics  simulation  component  of  the 
NPS  AUV  Integrated  Simulator. 

Appendix  C  is>  the  "C"  Language  Integrated  Production  System  (CLIPS)  expert 
system  source  code  for  NPS  AUV  Sonar  Classification  System.  This  program 
demonstrates  the  concepts  described  in  Chapter  VI  and  was  cowritten  with 
LCDR  Mark  A.  Compton  USN. 

Appendix  D  reproduces  an  as-yet-unpublished  paper  cowritten  with  Dr.  Yutaka 
Kanayama  that  provides  mathematically  rigorous  theoretical  detail  on  circle  world 
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shortest  path  planning  (Kanayama  Brutzman  91).  Appendix  E  provides  source  code 
implementation  of  the  circle  ^vorld  path  planning  algorithms. 

Appendix  F  describes  how  NPS  AUV  Integrated  Simulator  source  code  may  be 
obtained  via  Internet;  Graphics,  simulation,  sonar  classification  and  path  planning 
software  programs  are  all  available. 

Appendix  G  is  a  videotape  demonstration  of  pertinent  thesis  results.  Video  is 
essential  to  portray  the  power  and  effectiveness  provided  by  real-time  graphics 
simulation  and  scientific  visualization  techniques.  This  videotape  appendix  includes  a 
short  segment  on  the  NPS  AUV  presented  at  the  IEEE  Robotics  and  Automation 
Conference  1992  (Brutzman  Floyd  Whalen  92).  The  short  segment  was  coproduced 
with  CDR  Charles  A.  Floyd  USN  and  Mr.  Russell  Whalen,  and  benefited  from  the 
technical  advice  of  Dr.  Michael  J.  Zyda  and  Mr.  David  Pratt. 
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IL  AUV  RESEARCH  AT  THE  NAVAL  POSTGRADUATE  SCHOOL 

Naval  officers  and  civilian  scientists  at.NPS  are  working  on  an  AUV  that  is 
helping  change  the  nature  of  undersea  warfare.  Under.the  guidance  of  faculty  in  the 
Computer  Science,  Mechanical  Engineering  and  Electrical  Engineering  departments, 
NPS  students  have  developed  and  built  a  working  AUV  that  can  maneuver  and  operate 
submerged  and  unattended.  The  NPS  AUV  is  a  robotic  platform  for  basic  research, 
and  thesis  work  in  control  technology,  artificial  intelligence,  computer  visualization, 
and  systems  integration. 

A.  IMPORTANCE  OF  AUVs  IN  NAVAL  MISSIONS 

Unmanned  autonomous  submersibles  have  many  characteristics  that  make  them 
particularly  attractive  for  employment  in  naval  missions  (Polmar  91).  Vehicle 
autonomy  allows  independent  operation  in  changing  situations  without  a  tether  or  any 
direct  human  intervention.  Removing  the  need  for  a  crew  permits  routine  operation  in 
extremely  deep,  shallow,  or  tactically  hazardous  environments,  and  also  eliminates  the 
requirements  for  large  and  expensive  support  equipment.  Small  size  and  quiet 
propulsion  systems  result  in  unmatched  stealth.  The  relatively  low  cost  of  AUVs 
enables  the  acquisition  of  many  units  that  might  serve  as  force  multipliers  for  each  of 
the  Navy’s  warfare  communities. 

Many  robotic  vehicles  are  already  deployed  in  the  fleet  and  saw  action  in 
Operation  Desert  Storm.  Tomahawk  cruise  missiles  are  autonomous  weapons  that 
inflicted  heavy  damage  with  precision  accuracy  (Arthur  91).  Remotely  piloted 
vehicles  (RPVs)  flew  with  great  success  in  reconnaissance  and  naval  gunfire  support 
missions,  to  the  extent  that  Iraqi  soldiers  initially  made  RPVs  priority  targets  but  later 
surrendered  to  them  (Arthur  91)  (Burke  91).  Remotely  operated  vehicles  (ROVs) 
controlled  by  minesweepers  have  the  capability  to  send  back  live  underwater  video  in 
order  to  aid  in  the  hazardous  and  time-consuming  job  of  classifying  and  deactivating 
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mines  (Polmar  87).  Adding  autonomy  to  unmanned  vehicles  dramatically  increases 
their  independent  operating  range  and;  tactical  capabilities.  The  type  of  research  work 
being  conducted  at  NPS  is  of  fundamental  importance  in  making  these  performance 
breakthroughs  possible  for  unmanned  underwater  vehicles. 

B.  NPS  AUV  DESIGN  SPECIFICATION  SUMMARY 

The  design  and  construction  of  fhe  first  NPS  AUV  began  in  1987.  NPS  AUV  I 

was  a  two-foot  prototype  model  with  operational  screws  and  gyros  that  was  used  for 

the  investigation  of  model-based  maneuvering  controls,  including  the  automatic 

identification  of  significant  hydrodynamic  characteristics  (Healey  89).  The  full-scale 

vehicle  NPS  AUV  II  was  constructed  by  the  students,  technical  staff  and  faculty  of  the 

Mechanical  Engineering  department,  and  required  over  a  year  to  design  and  build 

(Good  89).  This  AUV  was  launched  (complete  with  traditional  champagne 

christening!)  by  the  Superintendent,  RADM  Ralph  W.  West,  Jr.  at  the  NPS  pool  on 

June  15,  1990.  According  to-Admiral  West,  a  submarine  officer, 

"The  AUV  is  one  of  the  new  technologies  that  will  play  a  major  role  in 
maintaining  the  effectiveness  of  our  fleet  units  as  the  threats  facing  us  become 
more  sophisticated  and  diverse."  (West  91) 

The  current  NPS  AUV  is  eight  feet  long  and  neutrally  buoyant,  displacing  387 
pounds  (Figure  2.1).  Its  overall  size  and  shape  is  comparable  to  a  dolphin.  Current 
vehicle  endurance  is  two  to  three  hours.  Maximum  speed  of  the  NPS  AUV  is  about 
two  knots.  The  NPS  AUV’s  turning  diameter  is  under  three  body  lengths,  designed  to 
be  ideal  for  maneuvering  in  the  large  NPS  swimming  pool  (Figure  2.2).  The  low 
noise  level  in  the  NPS  pool  allows  precise  testing  in  a  controlled  environment. 

Specific  test  missions  are  downloaded  into  the  NPS  AUV  using  a  poolside 
laptop  computer  (Figure  2.3).  NPS  AUV  posture  and  sonar  data  are  recorded  ten 
times  per  second  throughout  each  mission  and  then  immediately  uploaded  afterwards 
for  post-mission  analysis.  By  following  mission  software  commands,  the  NPS  AUV 
can  change  course  and  depth  without  any  external  direction.  The  low  speed  of  the 
vehicle  allows  divers  to  swim  nearby  and  safely  evaluate  its  performance  (Figure  2.4). 


Figure  2.1  The  NPS  AUV  is  an  eight  foot  long  submersible 

Open-ocean  testing  is  feasible  but  will  be  reserved  for  a  more  robust  follow-on 
vehicle. 

Initial  AUV  project  objectives  include  the  study  of  mission  planning,  navigation, 
collision  avoidance,  real-time  mission  control  and  replanning,  object  recognition, 
vehicle  dynamic  response  and  motion  control,  and  post-mission  data  analysis.  The 
primary  components  of  the  AUV  are  an  aluminum  hull,  fiberglass  sonar  dome,  four 
high-frequency  directional  sonar  transducers,  twin  counter-rotating  four-inch  propellers, 
lead-acid  batteries,  eight  plane  surfaces,  and  a  Gespac  computer  running  a  Motorola 
68030  processor  with  a  2MB  RAM  card.  Four  cross-body  tubes  have  been  included  to 
house  a  new  type  of  thruster  that  is  under  development.  When  completed,  these 
thrusters  will  allow  the  AUV  to  control  vehicle  posture  and  maintain  station  in  the 
presence  of  underwater  currents.  Figure  2.5  shows  a  general  schematic  of  the  AUV. 


Figure  22  The  NPS  swimming  pool  is  an  ideal  test  environment  for  the 
NFS  AUV 

C.  NPS  AUV  RESEARCH  OBJECTIVES 

The  scope  and  missions  of  the  AUV  project  are  not  restricted  by  any  specific 
programmatic  requirements.  Dr.  Robert  B.  McGhee,  Computer  Science  department 
chairman  states, 

"The  primary  purpose  of  our  AUV  work  has  always  been  to  support  student 
thesis  and  dissertation  research  without  restrictions."  (McGhee  91) 

Given  this  climate  of  academic  freedom,  a  myriad  of  topics  are  under  active 

investigation  and  are  resulting  in  numerous  advances  in  underwater  vehicle  technology. 

Already  over  fifty  theses  and  research  papers  have  been  published  about  the 

NPS  AUV. 

The  Mechanical  Engineering  department  has  primary  responsibility  for  the 
design,  construction  and  operation  of  this  submersible  robot.  Dr.  Anthony  J.  Healey  is 
the  Mechanical  Engineering  department  chairman  and  the  AUV  project  principal 


Figure  2.3 


Specific  test  missions  are  downloaded  into  the  NPS  AUV  using 
a  poolside  laptop  computer 


investigator.  He  has  assembled  a  diverse  group  of  over  two  dozen  faculty  and 
students,  making  this  project  the  largest  group  research  effort  at  NPS.  Dr.  Healey 
states, 

"The  most  important  aspect  of  the  project  is  to  involve  naval  officer  students  in 
the  development  of  control  technology  for  future  AUVs,  utilizing  their 
considerable  experience.  It  also  educates  them  in  the  potential  capabilities  of 
AUVs  and  the  technical  difficulties  yet  to  be  solved."  (Healey  91) 

Mechanical  Engineering  and  Electrical  Engineering  department  research  is 
currently  investigating  vehicle  stability  and  control,  modeling  submerged  dynamic 
behavior,  systems  integration  and  guidance/autopilot  design.  Of  immediate  interest  is 
integrating  low-power  components  such  as  ultrasonic  sonars,  steering  and  diving 
controllers,  guidance  circuitry,  Global  Positioning  System  (GPS)  receivers,  and 
miniaturized  inertial  measurement  units  currently  used  for  cruise  missile  navigation. 
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Figure  2.4  The  low  speed  of  the  NPS  AUV  allows  divers  to  swim  nearby 


and  evaluate  its  performance 

Computer  science  programmers  are  designing  the  "brains"  of  the  AUV.  This 
robot  must  be  able  to  transit  independently  to  the  desired  operating  area,  perform  a 
mission,  return  and  report  despite  any  unpredictable  tactical  situations  that  might 
occur.  Real-time  mission  planning  and  obstacle  avoidance  are  critical  aspects  of  these 
tasks.  The  basic  operations  of  the  AUV  resemble  those  found  in  naval  ships  and 
aircraft.  However,  piloting  and  tactical  functions  normally  performed  by  humans  must 
be  independently  handled  by  the  AUV’s  on-board  computer.  Not  surprisingly,  AUV 
software  organization  is  similar  to  a  ship’s  underway  watch  team.  Navigation, 
obstacle  avoidance,  data  collection  and  mission  execution  must  all  occur  continuously 
and  in  real  time  (Healey  et  al.  91).  Figure  2.6  is  a  block  diagram  of  the  AUV  mission 
execution  software  structure.  Most  of  the  functions  represent  tasks  normally 
performed  by  human  operators  on  submarines. 
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Figure  2.5  General  schematic  of  the  NPS  AUV.  Note  the  twin  screws,  four 
sonar  transducers  forward,  and  eight  plane  surfaces 

Real-time  three-dimensional  computer  graphics  simulation  of  the  AUV  is  being 
used  for  extensive  laboratory  evaluation.  New  hardware  and  software  can  be  tested 
prior  to  installation  and  operation,  minimizing  risk  and  saving  time  and  money. 
Replays  of  actual  data  recorded  by  the  AUV  can  be  used  for  visualization  of  remote 
environments  and  detailed  post-mission  data  analysis.  Silicon  Graphics  Inc. 
workstations  identical  to  those  used  for  special  effects  in  movies  such  as 
Terminator  2:  Judgement  Day  (Myers  91)  are  networked  together  and  provide  massive 
processing  power.  Because  the  AUV  hull  shape  is  similar  to  the  Swimmer  Delivery 
Vehicle  used  by  Navy  special  warfare  SEAL  teams,  a  sophisticated  mathematical 
model  was  already  available  for  simulator  use  to  accurately  recreate  vehicle  dynamic 
motion  and  response  characteristics  (Zyda  90).  This  dynamics  model  has  been 
validated  by  pool  testing.  NPS  AUV  graphics  simulations  can  use  actual  hydrographic 
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sounding  data  from  Monterey  Bay  provided  by  the  U.S.  Geological  Survey  to  show 
the  detailed  level  of  display  and  analysis  possible  (Figure  2.7)  (Jurewicz  91). 

Display  of  sonar  beams  and  the  sonar  environment  can  also  assist  operators  in 
evaluating  AUV  performance  (Figure  2.8).  Multiple  simulation  features  can  be 
combined  in  an  integrated  simulator.  Complete  visualization  of  the  ocean  environment 
and  AUV  system  response  permits  sonar  data  post-mission  analysis,  precise 
hydrodynamics  modeling  and  extensive  software  testing  to  be  performed  in  real  time. 

Artificial  intelligence  techniques  allow  a  robot  to  perform  tasks  normally 
requiring  human  intelligence.  Many  artificial  intelligence  methods  are  being 
developed  for  the  AUV.  Path  planning  and  spatial  reasoning  are  used  to  determine 
how  to  avoid  obstacles  and  optimally  travel  from  one  location  to  another.  Mission 
planning  enables  the  AUV  to  execute  an  ordered  mission,  while  mission  replanning 
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Figure  2.7  A  graphics  simulator  depicting  the  NPS  AUV  in  Monterey  Bay 
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Figure  2.8  Graphics  simulation  for  NPS  AUV  sonar  visualization 

flexibly  adjusts  to  changing  tactical  situations,  such  as  detection  of  an  unexpected 
obstacle  or  appearance  of  a  hostile  submarine.  Search  techniques  can  be  used  to  map 


minefields.  Expert  systems  can  ,  process  sonar  datamid  perform  object  recognition  and 
classification.  Neural  networks  can  boused  for  fault  diagnosis  and  mechanical  system 
control. 

Systems  integration  is  obviously  a  key  factor  in  the  construction  of  a  vehicle 
with  so  many  different  components.  There  are  a  large  number  of  hardware  and 
software  systems  that:  mutually  depend  upon  each  other  for  successful  operation. 
Similarly,  there  are  a  large  number  of  people  working  on  new  designs  in  each  of  these 
areas.  The  requirement  to  build  a  robust  and  independent  system,  means  that 
reliability,  redundancy  and  interoperability  must  be  thoroughly  considered  during  each 
phase  of  design  and  construction, 

D.  THE  FUTURE  OF  NAVAL  AUVs 

Potential  AUV  military  applications  are  limited  only  by  the  imagination.  Mine 
detection  and  minefield  mapping  might  be  completely  performed  by  single  or  multiple 
AUVs  in  direct  support  of  an  independent  submarine  or  surface  ship.  Multiple  AUVs 
could  quickly  clear  transit  lanes  for  fleet  deployment  or  amphibious  assault.  Dropping 
an  AUV  from  an  aircraft  could  be  a  quick  way  to  initiate  harbor  or  choke  point 
surveillance,  positioning  the  AUV  to  act  as  a  "bell  ringer"  to  warn  when  hostile  ships 
get  underway.  AUVs  have  the  ability  to  conduct  bottom  search  in  any  sea  state 
without  requiring  a  controlling  vessel  to  remain  on  station.  This  capability  would  keep 
critical  operations  such  as  high-value  object  searches  or  amphibious  landing 
preparations  in  progress  despite  difficult  weather  conditions.  Round-trip  or  one-way 
delivery  of  underwater  sensors  and  weapons  becomes  much  less  hazardous.  An  AUV 
configured  as  an  artificial  target  would  be  an  intelligent  and  realistic  adversary  during 
antisubmarine  warfare  exercises.  Software  control  of  autonomous  vehicles  permits 
relatively  rapid  and  inexpensive  upgrades  to  quickly  adapt  to  changing  enemy 
capabilities  and  new  mission  requirements.  Finally,  the  low  radiated  noise  and  small 
active  sonar  return  of  an  AUV  should  result  in  unmatched  stealth,  possibly  leading  to 
new  fleet  missions  that  are  not  currently  feasible. 


16 


Much  exciting  work  is  in  store  for  future  AUV  research  and  development 
Video  camera  recording,  ;  real-time  vision  processing  and  automatic  image 
interpretation  are  possible.  Green  laser  (543  nanometer)  range-finding  is  already 
operational  on  the  Monterey  Bay  Aquarium  Research  Institute  ROV  Ventana 
(Davis  91).  High  energy-density  power  sources  require  continued  development  in 
order  to  take  full  advantage  of  AUV  capabilities.  High  performance  sonar 
modifications  are  being  investigated  to  match  AUV  space  and  mission  requirements. 
Modular  connections  for  robot  arm  manipulators,  replaceable  packages  and  deliverable 
payloads  need  to  be  designed  for  flexible  support  of  all  potential  mission  requirements. 
Improved  computer  architectures  such  as  parallel  processing  transputers  will  allow 
simultaneous  accomplishment  of  many  tasks  in  real  time,  as  well  as  extend  the 
autonomy  and  artificial  intelligence  capabilities  of  these  vehicles.  High-bandwidth 
acoustic  modems  can  be  used  for  rapid  remote  communications  with  an  operating 
AUV.  Battle  group  commanders  should  have  the  ability  to  receive  remote  sensor 
reports  via  radio  uplinks  from  AUVs  on  station  and  send  mission  commands  in  return. 

Intelligent  mobile  robots  will  be  performing  many  missions  for  the  U.S.  Navy  in 
the  near  future.  Their  employment  as  extensions  of  our  ships,  submarines  and  aircraft 
will  become  commonplace.  The  imaginations,  technical  prowess  and  operational 
experience  of  officers  and  faculty  at  NPS  are  making  this  future  a  reality. 

In  undersea  warfare,  silence  and  nondetectability  are  the  most  important  factors 
in  achieving  stealth  and  tactical  advantage.  The  military  significance  of  autonomous 
underwater  vehicles  was  emphasized  during  a  recent  visit  to  NPS  by  a  Soviet  naval 
delegation.  A  small  group  of  senior  Soviet  officers  was  shown  the  NPS  AUV.  The 
group  looked  and  listened  politely.  The  Soviet  admiral  asked  only  one  question:  "Is 
it  quiet?" 

Indeed  it  is. 
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ffl.  INTEGRATED  SIMULATION  FOR  RAPID  AD V  DEVELOPMENT 
A.  ABSTRACT 

The  development  and  testing  of  Autonomous  Underwater  Vehicle  (AUV) 
hardware  and  software  is  greatly  complicated  by  vehicle  inaccessibility  during 
operation,  integrated  simulation  remotely  links  vehicle  components  and  support 
equipment  with  graphics  simulation  workstations,  allowing  complete  real-time, 
pre-mission,  p seudo-mission  and  post-mission  visualization  and  analysis  in  the  lab 
environment.  Integrated  simulator  testing  of  AUV  software  and  hardware  is  a  broad 
and  versatile  method  that  supports  rapid  diagnosis  and  robust  correction  of  system 
faults. 

Pre-mission  simulator  AUV  testing  permits  experimental  evaluation  of 
developmental  software.  Pseudo-mission  simulator  testing  of  AUV  processes  employs 
an  identical  laboratory  microprocessor  or  remote  communication  with  a 
testbench-mounted  operating  AUV,  permitting  end-to-end  testing  of  all  software  and 
hardware.  Post-mission  simulator  playback  of  recorded  telemetry,  sensor  data  and 
system  state  transitions  supports  in-depth  reenactment,  playback  and  analysis  of 
in-water  operational  results. 

High-resolution  three-dimensional  graphics  workstations  can  provide  real-time 
representations  of  vehicle  dynamics,  control  system  behavior,  mission  execution,  sonar 
processing  and  object  classification.  Use  of  well-defined,  user-readable  mission  log 
files  as  the  data  transfer  mechanism  allows  consistent  and  repeatable  simulation  of  all 
AUV  operations.  Examples  of  integrated  simulation  are  provided  using  the  Naval 
Postgraduate  School  (NPS)  AUV,  an  eight  foot,  387-pound  untethered  robot  submarine 
designed  for  research  in  adaptive  control,  mission  planning,  mission  execution,  and 
post-mission  data  analysis. 

The  flexibility,  connectivity  and  versatility  provided  by  this  approach  enables 
sophisticated  visualization  and  analysis  of  all  aspects  of  AUV  development.  Integrated 
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simulator  networking  is  recommended  as;  a  fundamental  requirement  for 
comprehensive  and  rapid  AUV  research  and  development. 

B.  INTRODUCTION 

1.  Problem  Statement 

Designing,  building  and  testing  ail  Autonomous  Underwater  Vehicle  (AUV) 
is  difficult  Unlike  most  other  mobile  robots,  AUVs  must  operate  unattended  and 
uncontrolled  in  a  remote  and  unforgiving  environment  Inaccessibility  greatly 
complicates  evaluation,  diagnosis  and  correction  of  AUV  system  faults.  In  order  to 
ensure  complete  reliability,  AUV  software  and  hardware  need  to  be  fully  tested  in  the 
laboratory  before  operational  deployment.  Such  important  testing  requirements  cannot 
be  met  using  only  a  standalone  AUV. 

2.  Motivation 

The  principal  motivation  driving  the  development  of  an  AUV  integrated 
simulator  is  to  meet  the  research  needs  of  the  large  academic  group  working  on  the 
Naval  Postgraduate  School  (NPS)  AUV.  Students  and  professors  have  diverse 
research  goals  that  are  often  forced  to  compete  for  access  to  vehicle  system  software 
and  limited  pool  test  time.  The  need  to  use  operational  software  running  on  actual 
NPS  AUV  hardware  is  a  particularly  important  requirement.  Lack  of  accessibility  to 
the  NPS  AUV  in  a  distributed  laboratory  environment  has  occasionally  prevented 
porting  new  software  applications  into  the  vehicle.  Pre-mission  validation  of  vehicle 
systems  response  to  new  software  has  been  similarly  limited  in  scope,  resulting  in 
several  operational  test  failures  and  frustrating  delays  in  development. 

The  integrated  simulation  approach  has  great  value  and  general 
applicability.  The  NPS  AUV  Integrated  Simulator  has  been  designed  to  support 
complete  scientific  visualization  of  actual  NPS  AUV  vehicle  performance.  The 
lessons  learned  while  building  this  integrated  simulator  have  proven  that  distributed 
research  can  be  effectively  accomplished  when  proper  network  connections  and 
data-passing  mechanisms  are  provided. 
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3.  Definition  and  Objectives  of  integrated  Simulation 

Integrated  simulation  is  defined  as-the  effective  networking  of  a 

three-dimensional  graphical  simulation  workstation  withan  AUV  microprocessor, 
appropriate  support  equipment  and  all  software  development  workstations.  Integrated 
simulation  allows  coordinated  utilization  of  computer  resources  for  maximum  realism 
and  effectiveness.  The  purpose  of  this  paper  is  to  demonstrate  the  use  of  integrated 
simulation  as  an  essential  approach  for  rapidly  designing,  developing  and  evaluating 
AUVs. 

An  AUV  integrated  simulator  remotely  links  vehicle  components  and 
support  equipment  with  '  graphics  simulation  workstations.  Networking  allows 
complete  pre-mission,  pseudo-mission  and  post-mission  visualization  and  analysis  in  a 
real-time  lab  environment.  Complete  integrated  simulator  testing  of  software  and 
hardware  supports  prompt  diagnosis  and  robust  correction  of  system  faults.  Figure  3.1 
shows  connectivity  for  a  sample  AUV  integrated  simulator  network  and  primary 
components. 

Pre-mission  simulator  testing  of  AUV  software  permits  experimentation  and 
preliminary  evaluation  of  developmental  software.  Pseudo-mission  testing  using  an 
identical  laboratory  microprocessor  or  remote  communication  with  an  actual  AUV 
permits  end-to-end  testing  of  all  AUV  software  and  hardware.  Post-mission  simulator 
playback  of  recorded  telemetry,  sonar' sensor  data  and  system  state  transitions  supports 
in-depth  reenactment,  playback  and  analysis  of  actual  operational  results. 

4.  Previous  Work 

Several  graphics  simulators  have  been  previously  developed  at  NPS  to 
support  AUV  research.  These  simulators  all  operate  on  Silicon  Graphics  Inc.  Iris 
graphics  workstations.  Seow  Meng  Ong  developed  a  simulator  that  remotely  networks 
an  Iris  workstation  with  a  Symbolics  Inc.  Lisp  machine  for  real-time  communication 
by  mission  planning  and  path  planning  software  (Ong  90)  (Zyda  90).  CDR  Thomas 
A.  Jurewicz  USN  developed  a  real-time  NPS  AUV  simulator  that  featured  a  complete 
hydrodynamics  model  and  bathymetric  survey  terrain  data  of  Monterey  Bay 
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Figure  3.1 


Integrated  simulator  network  physical  connectivity 


(Jurewicz  91)  (Zyda  91).  CDR  Charles  A.  Floyd  USN  extended  the  Jurewicz 
simulator  to  demonstrate  sonar  detection  and  collision  avoidance  software  (Floyd  91) 
(Floyd  Kanayama  Magrino  91).  MAJ  Ronald  B.  Byrnes  USA  and  LCDR  David  L. 
MacPherson  USN  utilized  the  network  capabilities  of  the  Ong  simulator  to  visually 
compare  hierarchical  and  subsumption  software  architectures  for  AUV  control 
(Byrnes  92). 

Other  less  complex  simulation  methods  have  also  been  used  for  NPS  AUV 
development.  Most  NPS  AUV  control  system  theses  have  analyzed  vehicle 
performance  parameters  individually  using  mathematics  support  packages  such  as 
MATLAB  (Mathworks  89),  forcing  researchers  to  visually  correlate  numerous 
two-dimensional  plots  of  telemetry  data  in  order  to  interpret  test  results. 

Other  underwater  vehicle  projects  have  also  used  offline  graphics 
simulation  as  a  design  tool.  As  an  example,  C.S.  Draper  Laboratories  has  a  large  and 
sophisticated  simulator  that  supports  the  development  of  the  Defense  Advanced 
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Research  Projects  Agency  (DARPA)  Unmanned  Underwater  Vehicle  (UUV).  This 
simulator  employs  sophisticated  computer  models  of /hydrodynamic. characteristics  and 
.individual  physical  component/responses.  Mission  software  is  loaded  on  a  separate 
mainframe  to  emulate  vehicle  multiprocessor  response.  The  simulator  does  not 
incorporate  actual  DARPA  UUV  multiprocessor  hardware  or  allow  direct  playback  of 
UUV  system  and  sensor  data  collected  in  the  water.  However  the  many  capabilities  of 
this  powerful  support  simulator  have  significantly  contributed  to  the  reliability  of  the 
DARPA  UUV,  allowing  successful  and  rapid  progress  along  an  ambitious  development 
schedule  (Pappas  91)  (Hale  91). 

All  of  these  simulation  approaches  successfully  demonstrate  the  concepts 
they  are  intended  to  evaluate.  However,  none  of  these  simulators  were  designed  to 
use  actual  vehicle  hardware  or  to  provide  general  extendability  to  support  every  aspect 
of  AUV  research. 

C.  AUV  DESIGN  AND  DEVELOPMENT  CONSIDERATIONS 

AUVs  are  complex  systems.  A  number  of,  design  and  employment  criteria 
unique  to  AUVs  must  be  considered  when  determining  integrated  simulator 
specifications. 

1.  AUV  Inaccessibility  During  Operation 

The  development  and  testing  of  AUV  hardware  and  software  is  greatly 
complicated  by  vehicle  inaccessibility  during  operation.  AUVs  are  designed  to  operate 
with  complete  independence  in  an  environment  that  makes  communication  and 
monitoring  difficult.  Vehicle  independence  design  constraints  leave  operators  unable 
to  monitor  performance,  diagnose  problems  or  override  failures.  This  inaccessibility  is 
perhaps  the  biggest  liability  inherent  in  AUV  testing  since  it  can  easily  lead  to 
catastrophic  failure  and  vehicle  loss.  Even  when  supervisory  control  is  possible 
through  use  of  a  tether  or  underwater  communications,  underwater  vehicle  systems 
must  be  robust  enough  to  recover  and  return  in  the  event  of  system  failures  combined 
with  communication  loss.  Integrated  simulation  can  fully  test  fault  tolerance  and 
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emergency  recovery  procedures  of  an  AUY  prior  to  risking  loss  of  communications 
during  independent  operation. 

2.  Reliability  is  Paramount 

Loss  of  an  AUV  due  to  internal  failure  or  inability  to  cope  with  an 
unpredictable  environment  is  unacceptable  due  to  the  current  high  cost  of  AUV 
construction  and  support.  Furthermore  if  an  AUV  is  employed  in  military  missions 
such  as  submarine  support  or  minefield  search,  human  lives  and  operational  success 
may  depend  on  complete  vehicle  reliability.  Thus  the  principal  requirement  for  any 
AUV  is  that  the  vehicle  operates  dependably  in  all  possible  scenarios  and  under  all 
possible  failure  conditions.  Pre-mission  verification  of  proper  AUV  performance  using 
an  integrated  simulator  is  the  only  way  to  ensure  complete  vehicle  integrity  and  verify 
strict  reliability  requirements  for  all  software  and  hardware  components. 

3.  Wide  Variety  of  Software  Process  Types 

The  highly  complex  behaviors  expected  of  AUVs  are  only  possible  when 
numerous  software  modules  are  written  to  handle  functions  such  as  path  planning, 
sonar  interpretation,  mission  control  etc.  Such  software  programs  can  be  considered 
artificial  intelligence  (AI)  applications  in  that  human  intelligence  might  otherwise  be 
required  to  perform  these  challenging  tasks.  It  is  important  that  these  high-level 
software  modules  are  able  to  fully  interact  with  each  other  for  proper  execution  and 
evaluation.  However  such  interaction  is  difficult  when  the  researchers  developing 
software  are  distributed  over  a  network.  Integrated  simulation  provides  full 
connectivity  between  research  software  modules  and  the  AUV  microprocessor. 
Integrated  simulation  also  provides  data-passing  mechanisms  that  permit  interprocess 
communication  regardless  of  the  various  host  operating  systems  or  programming 
languages  used. 


D.  INTEGRATED  SIMULATOR  SOFTWARE  ARCHITECTURE 

1.  Software  Engineering  Considerations 

Proper  design  of  an  AUV  integrated  simulator  addresses  many  requirements 
including  repeatability,  flexibility,  cost-effectiveness,  portability,  maintainability,  future 
growth  potential  and  ability  to  upgrade.  These  goals  can  be  met  by  following 
fundamental  software  engineering  principles  such  as  clearly  defining  software  module 
specifications  and  functional  descriptions.  Formally  defined  data  dictionary  entries, 
data  structures  and  spatial  coordinate  systems  are  also  important.  Specifications  must 
be  flexible  enough  to  support  future  improvements  and  comprehensible  enough  to  be 
rigorously  followed.  Frequent  and  thorough  communication  and  cooperation  among 
project  members  is  important  in  order  to  establish  formal  project  standards  and  ensure 
long-term  success. 

2.  Integrated  Simulator  Software  Architecture  Requirements 

Integrated  simulator  software  must  perform  a  large  number  of  tasks.  The 

AUV  must  be  modeled  using  some  simulated  components  (e.g.  control  surfaces, 
propellers,  gyrocompass)  together  with  actual  running  AUV  mission  software.  Vehicle 
physical  motion  and  behavior  can  be  provided  by  the  state  equations  of  a  dynamic 
response  model.  The  world  model  needs  to  include  stationary  obstacles,  mobile 
objects  and  the  sensor  interactions  expected  to  occur  as  the  vehicle  probes  the  external 
environment  model.  Developmental  AUV  processes  that  have  not  yet  been  ported  into 
the  vehicle  mission  software  need  to  have  some  way  of  interfacing  with  both  the  AUV 
microprocessor  and  the  simulation.  Operating  system  and  programming  language 
incompatibilities  should  not  be  an  impediment  to  AUV  software  developers.  Finally 
and  most  importantly,  a  powerful  graphics  workstation  must  render  an  external  view  of 
the  simulated  world  in  three  dimensions  with  full  functionality  and  real-time  response. 

3.  Simulated  and  Actual  Components 

Maximum  simulation  realism  is  provided  when  actual  AUV  components  are 
tested  end-to-end  in  the  laboratory.  For  example,  an  AUV  might  be  fixed  in  place  on 
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blocks  in  a  test  tank  while  a  test  mission  was  conducted;  Proper  activation  of  sonar, 
rudders,  diving  planes  and  propellers  would  provide  positive  indications  of  correct 
performance.  It  is  interesting  to  note  that  networking  a  test-tank  AUV  to  a  graphical 
simulator  can  give  evaluators  real-time  insight  into  what  the  vehicle  "thinks"  it  is 
doing.  However,  if  a  laboratory  AUV  microprocessor  is  used  instead  of  the  actual 
vehicle,  the  missing  vehicle  physical  components  must  be  separately  simulated.  Such 
simulation  is  accomplished  by  modular  substitution  of  mathematical  models  for  the 
missing  physical  components.  A  particular  benefit  of  this  approach  is  that 
AUV  software  testing  is  freed  from  direct  interaction  with  the  actual  AUV,  since  the 
vehicle  might  be  operating,  undergoing  repairs  or  otherwise  inaccessible.  The  logical 
relationships  between  AUV,  simulator,  laboratory  development  network  and  real-world 
environment  are  shown  in  Figures  3.2  and  3.3. 
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Integrated  simulator  logical  connectivity  using  actual  AUV 
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Figure  3.3  Integrated  simulator  logical  connectivity  using  laboratory  AUV 


4.  Data  Transfer  Mechanisms 

Data  transfer  mechanisms  are  a  critical  component  of  interprocess 
communication.  Two  file  types  and  two  data  transfer  mechanisms  are  considered: 
telemetry  replay  files,  high-level  object  files,  remote  file  transfer  and  stream  sockets. 

In  order  to  portray  and  replay  AUV  behavior,  telemetry  recorded  by  the 
vehicle  must  be  readable  by  the  integrated  simulator.  Typically  such  data  includes 
vehicle  position,  vehicle  orientation,  linear  and  rotational  velocities  or  accelerations, 
sensor  data  and  vehicle  state  information,  all  repeated  at  a  high  data  rate.  Telemetry 
replay  files  can  be  saved  by  an  AUV  for  post-mission  upload  or  transmitted  during 
operation.  These  files  can  also  be  read  (with  effort)  by  human  operators,  or  ported  as 
input  to  mathematics  support  packages  for  selective  analysis  of  system  parameters. 
However,  Figure  3.4  illustrates  the  difficulty  in  portraying 

three-dimensional  AUV  track  data  using  two-dimensional  time-versus-z  and  x-versus-y 
plots  (Compton  92). 
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Figure  3.4  Three-dimensional  AUV  track  evaluation  is  difficult  when  using 
multiple  two-dimensional  plots 

Use  of  well-defined  and  consistent  telemetry  replay  files  allows  repeatable 
simulation  of  all  AUV  missions.  Telemetry  replay  files  are  also  a  convenient  method 
for  new  mission  software  to  record  primary  aspects  of  AUV  behavior  during 
standalone  testing  for  later  visualization  on  the  integrated  simulator.  Figure  3.5  shows 
an  example  telemetry  replay  file  format. 
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<time> 

;  telemetry  data  point  time  . . 

<x>  <y> 

<z> 

;  vehicle  estimate  of  position 

«j»  <6> 

<\j/> 

;  measured  3D  orientation 

<p>  <q> 

<r> 

;  navigation  system  velocities 

<A  dive> 

<A  rudder> 

;  plane  surface  positions 

<rpm> 

<log  speed> 

;  ordered  and  measured  speed 

<sensor  data  fields> 

;  all  possible  sensor  returns 

<wildcard> 

;  extra  slot  for  mission-dependent  use 

Figure  3.5  Example  telemetry  replay  file  format 


High-level  object  files  allow  communication  of  symbolic  data  such  as 
position  of  objects,  object  classification,  operator  instructions  and  interprocess 
commands.  Keeping  such  data  in  plain  text  makes  them  readable  by  human  operators, 
individual  AUV  software  processes  and  the  integrated  simulator.  Optional  time 
parameters  on  each  command  line  allow  high-level  object  files  to  supplement 
telemetry  replay  files  for  synchronized  real-time  playback.  This  combination  of 
telemetry  replay  files  and  high-level  object  files  allows  simple  and  effective 
communication  of  all  possible  types  of  AUV  information.  An  example  high-level 
object  file  format  is  shown  in  Figure  3.6. 

File  transfer  is  the  fastest  and  easiest  way  to  record  and  communicate:  large 
amounts  of  data  over  a  distributed  research  network.  An  integrated  simulator  network 
must  be  able  to  transfer  telemetry  replay  and  high-level  object  files  between  all 
network  nodes. 

Once  a  file  transfer  capability  has  been  established,  stream  sockets  can  be 
implemented  if  transfer  of  individual  data  packets  is  desired  (Barrow  88).  Stream 
sockets  can  connect  all  processors  on  an  integrated  simulator  network,  allowing  direct 
interprocess  communication,  near  real-time  data  transfer  and  better  evaluation  of 
multiple  process  interaction. 
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Environment 

"wprldfilename 

ktt 

;  change  default  world  file 

AUV  <x> 

<y> 

<z> 

;  AUV  initial;  position 

Point  <x> 

<y> 

<z> 

;  Point  position  coordinates 

Segment  <xl> 

<yl> 

A 

'  rH 

N 

V 

<x2>  <y2> 

<z2>  ;  endpoint  coordinates 

Wall  <xl> 

<yl> 

<zl> 

<x2>  <y2> 

<z2>  ;  opposite  comers 

'  Cylinder  <x> 

<y> 

<z> 

<r>  <h> 

Mine  <x> 

<y> 

<z> 

<scale> 

[time  <t>] ;  time  optional 

Ship  <x> 

<y> 

<z> 

<scale> 

[time  <t>] 

Object  "filename" 

<x> 

<y>  <z> 

[time  <t>] 

Message  [time  <t>] 

...  free  format  text  here  ... 

;  messages  can  be  mission  log  outputs  or  interprocess  communication 

Figure  3.6 


Example  high-level  object  file  format 


5.  Distributed  Artificial  Intelligence  Considerations 

A  large  number  of  interrelated  AI  software  processes  are  required  for  an 
AUV  to  competently  perform  the  many  behaviors  required  of  an  independent 
submersible.  In  order  to  keep  up  with  demanding  mission  requirements,  these 
processes  must  be  capable  of  performing  in  real  time  and  in  parallel.  Similar  real-time 
and  parallel  processing  support  will  be  necessary  for  a  graphics  workstation  to  provide 
correspondingly  realistic  playback  and  interaction. 

Interprocess  communication  and  real-time  process  interaction  are  usually 
difficult  to  implement,  especially  if  multiple  user,  multiple  programming  language  or 
multiple  operating  system  bottlenecks  exist.  The  data  transfer  mechanisms  described 
previously  permit  complete  interaction  among  dissimilar  distributed  AI  applications, 
regardless  of  whether  these  applications  are  internal  or  external  to  the  AUV.  This 
straightforward  approach  allows  complete  user  and  simulator  accessibility  to 
intermediate  process  outputs. 
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E.  THREE-DIMENSIONAL  GRAPHICS  SIMULATION 

ffigh-resoiiition  three-dimehsibnal  graphics  workstations  provide  realistic 
representations  of  vehicle  dynamics,  control,  system  ibehavior,  mission  execution,  sonar, 
processing,  and  object  classification. 

L  Realistic  Object  Rendering  and  Real-Time  Motion 

The  primary  graphics  requirement  for  an  integrated  simulator  is  realistic 
rendering  and  movement  of  virtual  objects  in  real  time.  This  capability  is  essential  for 
visualizing,  an  AUV’s  interaction  with  an  underwater  world  , in  order  to  fully  evaluate 
the  proper  operation  of  complex  AUV  software  and  hardware.  Numerous,  graphics 
techniques  can  be  used  to  provide  a  believable  graphics  display,  ranging  from  drawing 
simple  polygons  to  overlaying  complex  textures.  Realistic  portrayal  of  all  objects  in 
an  underwater  world  allows  intuitive  and  thorough,  analysis  of  large  amounts  of  AUV 
data. 

Maintaining  a  real-time  playback  capability  is  important  for  realistically 
rendering  AUV  interaction  with  physical  objects.  The  graphics  simulator  program 
must  be  able  to  quickly  refresh  complex  screens  in  order  to  visually  present  large 
amounts  of  data.  Local  empirical  studies  show  that  a  6  Hz  screen  update  rate  and 
input  device  response  loop  are  the  minimum  requirements  for  simulator  screen  motion 
to  appear  smooth  and  realistic  during  operator  interaction.  Frame  rates  of  20-30  Hz 
may  be  needed  for  realistic  illusion  of  rapid  motion  (Brooks  88).  Speed  can  be 
increased  and  graphics  pipeline  loading  reduced  through  simplified  object  geometry, 
simplification  of  lighting  models,  simulator  source  code  optimization  and  graphics 
performance  tuning  techniques. 

2.  Physical  Modeling 

All  AUV-related  physical  processes  can  be  mathematically  modeled  with  a 
high  degree  of  accuracy.  Vehicle  physical  response  can  be  predicted  using  state 
equations,  positional  constraints,  inverse  kinematics  and  dynamics  (Jurewicz  90) 
(Thalmann  90)  (Badler  91).  Sonar  acoustic  behavior  can  be  modeled  with  increasingly 
complex  levels  of  detail  in  order  to  meet  both  realism  and  system  playback 
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requirements  (Etter  91).  Individual  AUV  hardware  components  can  be  simulated 
using  control  system  models  of  transient  and  steadyrstate  response.  Object  motion  is 
adequately  modeled  using;  simple  kinematics.  Object  positions  can  be  easily  updated 
whenever  more  recent  correlated  sonar  data  becomes  available.  In  general,  physical 
modeling  is  less  processor-intensive  than  graphics  rendering  and  adds  no  apparent 
overhead  to  graphics  workstation  response  when  properly  parallelized  (Jurewicz  90) 
(Zyda  91). 

3.  Sonar  and  Sensor  Visualization 

Sonar  data  is  often  difficult  to  visualize  since  acoustic  beam  and  ray  path 
behavior  is  very  different  from  our  vision-based  perceptual  expectations.  Sonar 
remains  the  primary  sensor  used  for  intermediate  and  long  range  underwater  detection. 
Sonar  can  also  be  quite  effective  when  used  for  short  range  detection,  object  feature 
extraction  or  measurement  of  object  characteristics  such  as  doppler  or  frequency 
response.  Color  graphics  visualization  can  portray  the  real-time  behavior  of  sonar 
beams  in  three  dimensions,  allowing  AUV  designers  to  troubleshoot  complex 
problems,  optimize  vehicle  sensor  performance  and  better  understand  how  an  AUV  is 
interacting  with  the  environment  (Brutzman  Compton  92)  (Brutzman  92) 

(Compton  91).  Other  types  of  sensors  such  as  laser  rangefinders  can  also  be 
displayed.  Sensor  visualization  capabilities  are  valuable  features  for  an  integrated 
simulator, 

F.  INTEGRATED  SIMULATOR  HARDWARE  ARCHITECTURE 

1.  Workstation  Compatibility 

There  are  surprisingly  few  hardware  constraints  on  the  individual 
workstations  making  up  the  distributed  network  portion  of  an  integrated  simulator.  A 
variety  of  normally  incompatible  operating  systems  and  programming  environments 
may  be  used  as  long  as  network  connections  provide  a  open  data  transfer  path.  Even 
application  source  code  may  be  in  a  language  foreign  to  the  AUV.  For  example,  a 
high-level  language  (e.g.  Lisp,  CLIPS  or  Prolog)  may  be  used  for  rapid  prototyping 
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and  initial  development.  Testing  is  then  accomplished  using  interprocess 
communication  and  real-time  data  transfer  of  high-level  object  ftie  inforriiatioh  with, 
the  AUV.  After  initial, process  testing  is  complete,  working  high-level  language  code 
can  be  translated  and  ported  into  the  native  language  of  the  AUV  (e.g.  ANSI  C).  This 


open  architecture  approach  allows  great  flexibility  and  maximum  use  of  available 
resources. 


2.  External  Network  Connectivity 

Fully  networked  connections  between  all  major  support  components  of  the 
AUV  is  essential  to  provide  a  responsive  research  environment.  Additional  external 
network  connections  will  further  extend  AUV  integrated  simulator  capabilities.  For 
example,  laboratory  data  transfers  over  a  wide-area  network  or  Internet  allow  joint 
AUV  research  over  long  distances.  For  another  example,  actual  telemetry  replay  files 
can  be  transferred  from  a  moored  AUV  via  modem  or  radio  link  for  immediate  remote 
replay,  analysis  and  verification.  Such  capabilities  are  particularly  important  when  an 
AUV  is  deployed  at  great  distances  from  support  laboratories  and  immediate  analysis 
of  collected  information  is  necessary. 


G.  IMPLEMENTATION,  EVALUATION  AND  EXPERIMENTAL  RESULTS 
An  integrated  simulator  has  been  implemented  for  the  NPS  AUV  (Brutzman  92). 
This  section  describes  the  primary  components  and  key  features  of  the  NPS  AUV 
Integrated  Simulator. 

1.  NPS  AUV  Vehicle  Description  and  Sonar  Characteristics 

Naval  officers  and  civilian  scientists  at  NPS  are  conducting  active  research 
using  an  AUV  designed  and  constructed  at  the  school.  The  NPS  AUV  is  used  for 
basic  research  and  thesis  work  in  control  systems  technology,  artificial  intelligence, 
scientific  visualization  and  systems  integration.  Specific  NPS  AUV  project  objectives 
include  the  study  of  mission  planning,  navigation,  collision  avoidance,  real-time 
mission  control,  replanning,  object  recognition,  vehicle  dynamic  motion  control,  and 
post-mission  data  analysis  (Healey  91)  (Brutzman  Compton  91). 
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The  NPS  AUV  is  eight  feet  long  and  neutrally  buoyant,  displacing  387 
pounds  with  overall  size  and  shape  comparable  to  a  small  dolphin.  Current  vehicle 
endurance  is  two  to  three  hours.  Maximum  speed  of  the  NPS  AUV  is  about  two 
knots.  The  NPS  AUV  turning  diameter  is  under  three  body  lengths,  designed  to  be 
ideal  for  maneuvering  in  the  large  NPS  swimming  pool.  The  NPS  pool  allows  .precise 
testing  in  a  quiet,  controlled  environment.  Open-ocean  testing  is  feasible  but  is  being 
reserved  for  a  more  robust  follow-on  vehicle.  Video  clips  showing  normal  NPS  AUV 
operation  are  available  in  (Brutzman,  Floyd,  Whalen  92)  and  (Brutzman  92). 

The  primary  components  of  the  NPS  AUV  are  an  aluminum  hull,  fiberglass 
sonar  dome,  four  high-frequency  directional  sonar  transducers,  twin  counter-rotating 
four-inch  propellers,  lead-acid  batteries,  eight  plane  surfaces,  and  a  Gespac  computer 
running  a  Motorola  68030  processor  with  a  2  MB  RAM  card  under  the  OS-9  operating 
system.  Figure  3.7  shows  a  general  schematic  of  the  NPS  AUV. 

Four  PSA-900  Programmable  Sonar  Altimeters  made  by  Datasonics  Inc.  are 
orthogonally  fixed  in  the  nose  of  the  NPS  AUV  pointing  directly  ahead,  downward 
and  to  port  and  starboard.  These  transducers  are  fixed  frequency  and  ultrasonic,  each 
at  approximately  200  Khz.  Sonar  range  gate  is  selectable  at  30  m  or  300  m,  and  pulse 
length  is  350  |is.  Normal1  pulse  repetition  rate  is  10  Hz.  Sonar  beamwidth  is  seven 
degrees  and  range  resolution  is  1  cm  at  30  m. 

2.  NPS  AUV  Integrated  Simulator 

The  NPS  AUV  Integrated  Simulator  has  been  developed  to  support 
NPS  AUV  research  and  demonstrate  each  of  the  concepts  described  in  this  paper 
(Brutzman  92). 

High-level  NPS  AUV  software  processes  are  initially  developed  and  tested 
on  the  Unix-based  computer  science  department  network.  These  processes  can  now  be 
ported,  compiled,  linked  and  loaded  on  a  Gespac  VME-bus  68020  or  68030 
microprocessor  running  under  the  OS-9  operating  system.  Gespac  microprocessors  are 
used  both  on  the  NPS  AUV  and  on  a  separate  networked  laboratory  AUV.  The 
laboratory  AUV  includes  68020  microprocessor  and  I/O  cards,  a  monitor  terminal, 
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Figure  3.7 


General  schematic  of  NPS  AUV  to  scale 


Ethernet  network  connections  and  a  networked  IBM-compatible  support  PC  that 
includes  an  OS-9  "C"  language  cross-compiler.  The  laboratory  AUV  also  has 
additional  hardware  card  slots  in  order  to  test  new  hardware  components  and  new 
vehicle  software. 

Graphics  simulation  using  the  NPS  AUV  Integrated  Simulator  is  just 
beginning  to  be  used  for  laboratory  evaluation  of  software  that  will  run  in  the  AUV 
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proper.  New  hardware  and  software  can  be  rapidly  tested  prior  to  installation  and 
operation  in  the  NPS  AUV,  minimizing  vehicle  risk  while  saying  time  and  money. 
Replays  of  actual  data  recorded  by  the  NPS  AUV  can  be  used  for  visualization  of 
remote  environments  and  detailed  post-mission  data  analysis.  Connection  of  software 
development  workstations  with  the  NPS.  AUV  Integrated  Simulator  accelerates  the 
operational  deployment  of  high-level  mission  software. 

The  NPS  AUV  Integrated  Simulator  control  panel  has  been  written  using 
NPS  Panel  Designer  software,  making  it  quickly  modifiable  and  extendable 
(King  Prevatt  91).  The  graphic  simulator  user  interface  permits  precise  control  of 
viewpoint  and  reference  point,  lighting  and  rendering  functions,  object  positions, 
real-time  mode,  high-level  object  file  recall,  individual  object  control  and  playback  of 
telemetry  replay  files.  The  NPS  AUV  Integrated- Simulator  control  panel  is  shown  in 
Figure  3.8.. 


Figure  3.8  Control  panel  for  the  NPS  AUV  Integrated  Simulator 


Because  the  AUV  hull  shape  is  similar  to  the  original  Swimmer  Delivery 
Vehicle  used  by  U.S.  Navy  SEAL  teams,  a  sophisticated  mathematical  model  is 
already  available  for  simulator  use  to  accurately  recreate  vehicle  dynamic  motion  and 
response  characteristics  (Jurewicz  90).  The  hydrodynamics  model  state  equations 
contain  approximately  120  coefficients  that  continue  to  be  improved  and  verified  by 
pool  testing  and  ongoing  thesis  work. 

In  order  to  display  a  variety  of  sonar  data,  multiple  objects  can  be  displayed 
on  the  graphics  workstation.  Implemented  object  primitives  include  AUV,  point,  line, 
wall,  mine  and  cylinder.  These  objects  can  be  graphically  displayed  simultaneously 
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with  original  telemetry  replay  data  in  order  to  analytically  visualize  the  validity  and 
usefulness  of  various  sonar  classification  techniques.  Objects  can  be  independently 
manipulated  and  positioned.  An  optional  time  slot  for  each  object  allows  them  to 
appear  only  when  appropriate  during  synchronized  playback  of  telemetry  files. 
Additional  advanced  graphics  techniques  can  quickly  be  added  to  the  baseline  graphics 
simulation  program. 

3.  Silicon  Graphics  IRIS  Workstation  Capabilities 

The  NPS  AUV  Integrated  Simulator  uses  a  Silicon  Graphics  Inc. 

Iris  4D/240VGX.  This  graphics  workstation  has  48  bit  color,  24  bit  Z-buffering  and 
four  parallel  25  Mhz  20  MIPS  processors  that  together  can  process  1  M  vectors,  1.1  M 
triangles  or  180  K  polygons  per  second  (Gorey  91).  Other  slower  IRIS  workstations 
are  also  available  for  use,  including  a  remote  workstation  in  the  ^Mechanical 
Engineering  Department  adjacent  to  the  NPS  AUV  support  laboratory.  All  graphics 
workstations  are  connected  by  local  or  wide  area  networks.  Real-time  playback  of 
telemetry  data  is  automatically  adjusted  to  take  maximum  advantage  of  the  current 
graphics  workstation  processing  power,  producing  realistic  screen  tdisplays  regardless 
of  which  model  graphics  workstation  is  used. 

4.  Laboratory  AUV  Simulation 

A  primary  objective  of  integrated  simulation  is  to  run  operational  software 
on  a  laboratory  version  of  the  AUV  microprocessor.  The  NPS  AUV  Integrated 
Simulator  includes  an  identical  Gespac  computer  running  a  Motorola  68030  under  the 
OS-9  operating  system.  Added  to  this  computer  are  interface  cards  for  a 
VT220  monitor  and  keyboard  for  external  control,  serial  connection  to  a  PC,  and 
Ethernet  connection  to  the  NPS  computer  science  department  network.  Full 
connectivity  is  thus  provided  to  all  developmental  workstations  of  interest  as  well  as 
the  Campus  Wide  Network  and  Internet.  Since  OS-9  is  a  multiprocess  real-time 
system,  multiple  users  can  access  the  Gespac  AUV  microprocessor  simultaneously. 

Unmodified  operational  NPS  AUV  software  is  able  to  run  successfully  on 
the  laboratory  AUV  microprocessor,  and  telemetry  data  files  are  properly  saved  during 


36 


each  run.  Telemetry  data  files  have  been  successfully  transferred  over  the  network 
and  played  back  on  the  Ms  graphics  workstation.  Although  missing  NPS  AUV 
hardware  such  as  sonar  and  plane  surface  response  has  not.yet  been  simulated, 
successful  visualization  of  the  laboratory  test  runs  has  proven  the  feasibility  of  the 
integrated  simulation  approach.  Functional  AUV  software  and  hardware  is  now 
directly  available  to  all  NPS  AUV  researchers  for  experimentation  and  evaluation  prior 
to  in-water  testing. 

H.  ADDITIONAL  APPLICATIONS 

Several  applications  were  implemented  concurrently  with  the  NPS  AUV 
Integrated  Simulator  that  successfully  demonstrate  the  usefulness  of  integrated 
simulation  in  support  of  high-level  AUV-related  AI  research. 

1.  Sonar  Classification  Application 

The  NPS  AUV  Sonar  Classification  System  uses  outputs  from  simple  active 
sonars  to  classify  detected  underwater  objects  (Brutzman  Compton  Kanayama  92) 
(Brutzman  92).  Figure  3.9  shows  sample  sonar  classifications. in  the  NPS  pool 
displayed  using  the  NPS  AUV  Integrated  Simulator.  Scientific  visualization 
techniques  permitted  rapid  and  precise  development  of  geometric  analysis  techniques 
and  classification  heuristics,  resulting  in  successful  completion  of  the  NPS  AUV  Sonar 
Classification  System. 
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Figure  3.9  Integrated  simulator  screen  display  of  the  NPS  pool,  AUV  track 
and  all  active  sonar  classifications 


2.  Circle  World  Path  Planning  Application 

Optimal  path  planning  is  an  important  area  of  AUV  research.  Displaying 
and  viewing  paths  and  obstacles  without  restrictions  allows  the  algorithm  designer  to 
evaluate  his  results  in  the  most  comprehensive  and  challenging  manner  possible. 
Additionally,  subtle  difficulties  that  might  be  obscured  by  two-dimensional  projections 
are  clearer  and  easier  to  evaluate  when  shown  in  three-dimensions.  A  circle  world 
path  planner  has  been  developed  that  finds  shortest  paths  around  circular  or  cylindrical 
obstacles  (Brutzman  92).  Figure  3.10  shows  how  shortest  path  planning  results  can  be 
portrayed  in  three  dimensions  using  the  NPS  AUV  Integrated  Simulator. 
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Figure  3.10  Integrated  simulator  three-dimensional  representation  of  circle 
world  obstacles  and  shortest  path  in  the  NPS  pool 


3.  Minefield  Search  Application 

Another  application  benefiting  from  integrated  simulation  is  an  AUV 
minefield  search  planner  (Compton  92).  A  three-dimensional  open-ocean  minefield 
model  is  optimally  searched  and  mapped  using  a  dynamic  search  strategy.  AUV 
search  track  and  vehicle  posture  are  recorded  in  a  simplified  telemetry  replay  file, 
while  waypoint  objectives  and  detected  mines  are  recorded  in  a  separate  high-level 
object  file.  Synchronized  playback  of  these  files  allows  complete  visualization  of  the 
complex  path  taken  by  the  AUV  as  well  as  the  numerous  objects  detected,  shown  in 
Figure  3.11.  Note  that  vehicle  track  is  much  easier  to  visualize  than  in  Figure  3.4, 
particularly  since  the  simulator  user’s  viewpoint  can  be  panned  over  and  around  the 
track  data. 
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Figure  3.11  Integrated  simulation  display  of  AUV  minefield  search 

I.  ADDITIONAL  APPLICABILITY,  LIMITATIONS  AND  FUTURE  WORK 

1.  Comparison  of  Theoretical  and  Empirical  Data 

Three-dimensional  visualization  techniques  are  well  suited  for  making 
meaningful  comparisons  between  large  abstract  data  sets.  Such  comparisons  can 
significantly  aid  the  operator  in  evaluating  small  errors  in  mathematical  models  or 
system  control  software.  For  example,  predicted  vehicle  track  for  a  given  control 


systems  algorithm  could  be  spatially  superimposed  over  actual  test  track  data. 
Coefficients  in  the  prediction  model  can  then  be  incrementally  adjusted  until 
theoretical  behavior  matches  actual  performance.  A  similar  visualization  approach  was 
used  with  great  success  while  determining  precise  heuristics  for  object  classification 
using  recorded  active  sonar  data  (Brutzman  Compton  Kanayama  92)  (Brutzman  92). 
Direct  comparison  of  theoretical  and  empirical  data  is  a  powerful  diagnostic  tool  that 
can  be  used  to  improve  theoretical  formulations  as  well  as  vehicle  implementations. 

2.  Limitations  to  Integrated  Simulation 

The  primary  limitation  on  integrated  simulation  realism  is  graphics 
workstation  speed  and  capability.  Many  graphics  workstations  can  generate 
photorealistic  images  but  are  unable  to  rapidly  reproduce  a  series  of  images  in  real 
time.  The  competing  requirements  between  rendering  accuracy  and  adequate  frame 
rate  will  always  require  design  tradeoffs  by  the  graphics  programmer.  Silicon 
Graphics  Inc.  workstations  use  the  GL  Graphics  Library,  which  is  a  good  graphics 
programming  choice  due  to  the  numerous  graphics  techniques  provided,  code 
optimization,  portability  to  other  platforms  and  open  licensing  availability.  Graphics 
workstation  capabilities  are  probably  the  most  critical  consideration  in  integrated 
simulator  design. 

The  local  area  network  (LAN)  used  to  connect  integrated  simulator  nodes 
should  be  reliable,  have  adequate  throughput  and  allow  addition  or  removal  of  nodes 
with  little  difficulty.  Ethernet-based  LANs  are  adequate  for  NPS  AUV  Integrated 
Simulator  requirements  and  also  provide  gateway  connectivity  to  Internet.  It  should  be 
noted  that  under  most  network  protocols  socket  stream  packet  delivery  order  is  not 
guaranteed  and  timing  of  packet  delivery  is  somewhat  unpredictable.  Processes  that 
use  socket  stream  data  should  be  flexible  and  not  tied  to  hard  real-time  requirements. 

Computer  security  is  a  consideration  if  sensor  data  or  mission  software  is 
proprietary  or  classified.  The  use  of  plain  text  for  telemetry  replay  files  and  high-level 
object  files  permits  the  use  of  encryption  protocols  during  transfer.  Encrypting  files  is 
a  simple  technique  that  imposes  minimal  processing  overhead.  Individual  nodes  on 
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the  integrated  simulator  network  will  require  standard  security  precautions  . against 
unauthorized  remote  access. 

3.  Future  Use  of  Integrated  Simulation 

Integrated  simulation  provides  development  benefits  to  all  types  of  remote 
vehicles,  regardless  of  whether  a  communications  tether  is  present  or  remote  control 
by  human  operators  is  required.  Integrated  simulation  not  only  solves  a  number  of  the 
problems  that  degrade  robot  implementation,  but  also  provides  tools  to  work  on 
practical  system  engineering  and  integration  problems  that  previously  were  too 
difficult  to  address.  The  authors  hope  that  widespread  incorporation  of  integrated 
simulation  techniques  will  improve  the  accessibility,  intelligibility  and  progress  rate  of 
mobile  robot  research. 

J.  CONCLUSIONS 

Integrated  simulation  allows  all  AUV  systems  to  be  tested  in  a  timely  and 
complete  manner.  The  flexibility  and  connectivity  provided  by  this  approach  enables 
sophisticated  visualization  and  complete  analysis  of  all  aspects  of  AUV  development. 
Integrated  simulator  networking  is  recommended  as  a  fundamental  requirement  for 
comprehensive  and  rapid  AUV  research  and  development. 
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IV.  NPS  AUV  INTEGRATED  SIMULATOR  DESIGN  SPECIFICATIONS 
This  chapter  provides  details  about.design  specifications  particular  to-the 
NPS  AUV  and  the  NPS  AUV  Integrated  Simulator.  Specifications  must  include  both 
vehicle  and  integrated  simulator  requirements  for  compatibility.  Users  and 
programmers  need  to  comply  with  or  formally  improve  design  specifications  in  order 
to  maintain  forward  and  backward  compatibility  throughout  the  effective  lifetime  of 
the  NPS  AUV  research  project.  A  valid  and  effective  data  dictionary  defined  in 
(Floyd  91)  is  used  for  all  data  types  defined  in  this  thesis. 

A.  NPS  AUV  ACTIVE  SONAR  SYSTEM 

The  current  NPS  AUV  active  sonar  uses  four  ultrasonic  directional  beams 
pointed  ahead,  down  and  90°  to  port  and  starboard.  Individual  transducers  are 
mounted  on  adjustable  semicircular  frames  that  can  allow  all  beams  to  be  directed 
forward.  However  the  dimensions  of  the  NPS  pool  and  the  30  m  range  gate  of  the 
transducers  make  orthogonally  oriented  transducers  an  optimal  approach  for  sensing 
multiple  walls  and  targets  simultaneously  (Figure  4.1).  The  NPS  AUV  active  sonar 
characteristics  are  further  described  in  Chapter  VI  and  (Floyd  91). 

Several  problems  have  handicapped  NPS  AUV  sonar  performance.  Faults  in  the 
signal  processing  electronics  have  prevented  simultaneous  utilization  of  multiple  sonar 
transducers.  Automatic  averaging  of  sonar  ranges  at  the  board  level  has  introduced 
minor  range  errors  and  reduced  the  discrimination  capability  of  the  transducers  against 
small  targets.  The  strongly  reflective  walls  and  shallow  depth  of  the  NPS  pool  create 
a  high  reverberation  environment.  Despite  these  significant  difficulties,  adequate  sonar 
classification  results  have  been  obtained  inside  the  NPS  pool. 

In  a  real  sense,  the  independence  and  capabilities  of  any  mobile  robot  (or  even 
manned  submarine)  are  constrained  by  the  quantity  and  quality  of  sensor  data  available 
about  the  external  world.  Correction  of  current  NPS  AUV  sonar  limitations  is 
essential  if  more  accurate  vehicle  control  and  higher  level  behaviors  are  to  be  achieved. 
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B.  NPS  POOL  COORDINATE  SYSTEM 

Several  theses  have  graphically  modeled  the  NPS  swimming  pool  or  utilized 
AUV  data  collected  in  the  NPS  pool.  Unfortunately  most  of  these  efforts  are 
incompatible  because  no  standard  pool  coordinate  system  has  been  established.  In 
order  for  test  results  to  be  understandable  and  repeatable  throughout  the  life  of  the 
NPS  AUV  program,  a  standardized  NPS  Pool  Coordinate  System  is  defined  here. 

Numerous  competing  criteria  were  resolved  when  defining  this  coordinate 
system,  especially  differences  between  coordinate  systems  used  by  graphics  simulation 
programs.  Advantages  of  the  NPS  Pool  Coordinate  System  are  as  follows: 

•  all  pool  coordinates  positive  and  units  in  feet 

•  surface  depth  z  equals  zero,  increasing  depth  corresponds  to  increasing  z 
(indicated  on  diagram  by  tail  of  z-axis  arrow) 

•  NPS  AUV  data  file  coordinates  become  standardized  for  readability  and 
future  reference 

•  vehicle  position  and  posture  terminology  are  standardized 

•  right-hand  rule  relationship  between  all  three  axes  maintained 

•  compatible  with  vehicle  coordinate  system  and  Euler  angle  definitions 

•  typical  start  points  and  normal  operator’s  perspective  are  near  pool  origin 

•  AUV  reference  point  between  center  of  gravity  and  center  of  buoyancy 

•  angle  orientations  and  coordinate  positions  are  directly  compatible  with  the  most 
prevalent  robotics  conventions,  Dr.  Kanayama’s  spatial  reasoning  function 
definitions  and  standard  "C"  language  trigonometric  function  calls 

•  vehicle  headings  are  measured  in  clockwise  direction  as  are  conventional 
compass  headings  that  are  familiar  to  naval  officers 

•  NPS  Pool  Coordinate  System  simultaneously  combines  Cartesian  coordinate 
plane  characteristics,  Euler  angles  and  right-hand  rule,  advantages  that  are  not 
possible  with  any  other  spatial  representation 

Disadvantages  of  this  coordinate  system  are  as  follows: 

•  similarity  to  Cartesian  plane  is  only  evident  from  a  perspective  looking  up  to  the 
pool  surface  from  below,  thus  axis  orientations  may  initially  be  counterintuitive 

The  NPS  Pool  Coordinate  System  is  shown  in  Figure  4.2. 
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NPS  Pool  Coordinate  System 
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Ci  NPS  AUV  TELEMETRY  REPLAY  FILE  FORMAT 

Telemetry  replay  files  are  a  critical  component  of  an  integrated  simulator.  These 
files  are  used  to  record  in- water  test  data  as  well  as  portray  expected  vehicle  behavior 
from  offline  simulation  programs.  Numerous  variant  formats  exist  for  NPS  AUV 
telemetry  replay  files  recorded  to  date.  This  variability  leads  to  confusion  and 
incompatibility  that  worsens  as  an  ever-growing  number  of  unique  telemetry  replay 
files  become  available. 

The  primary  reason  behind  the  current  plethora  of  telemetry  data  file  formats  is 
that  different  NPS  AUV  evaluation  runs  tend  to  test  different  hardware  or  software 
components.  Different  tests  have  correspondingly  different  data  logging  requirements. 
A  telemetry  replay  file  format  standard  has  to  be  flexible  to  support  varying  data 
logging  requirements.  The  current  NPS  AUV  telemetry  replay  file  format  is  defined 
in  Figure  4.3. 

NPS  AUV  telemetry  is  recorded  at  a  10  Hz  data  rate,  allowing  precise 
measurements  of  varying  position,  posture  and  sonar  values.  A  single  wildcard  data 
slot  can  take  advantage  of  this  rapid  update  rate  to  allow  recording  of  multiple  test 
parameters.  For  example,  suppose  a  special  test  needed  to  record  both  left  and  right 
commanded  motor  RPM.  Alternate  wildcard  slots  can  be  used  for  the  two  parameters 
and  a  5  Hz  data  logging  rate  for  each  is  maintained.  Another  method  might  record 
three  3-digit  numbers  by  multiplying  each  by  an  appropriate  powers  of  ten.  For 
example,  the  numbers  200,  300  and  400  can  be  saved  as  the  single  wildcard  slot  value 
"200,300,400.00",  permitting  three  parameters  to  be  logged  at  the  full  10  Hz  data  rate. 
Thus  this  newly  standardized  telemetry  replay  file  format  has  built-in  flexibility  to 
allow  compatible  recording  of  variable  data  logging  requirements. 
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;  Telemetry  track  data  entries  are  uniquely  identified  by  time  value 

<time>  =  REAL  ;  time  of  sonar  ping 

<x>,  <y>,  <z>  =  REAL  ;  AUV  position  at  time  of  ping 

«J»,  <0>,  <\}/>  =  REAL  ;  roll,  elevation,  azimuth  angles 

;  vehicle  posture  at  time  of  ping 

<p>,  <q>,  <r>  =  REAL  ;  vehicle  posture  angular  rates  of  change 

;  from  navigational  gyros 

<A  dive>  =  REAL  ;  Ordered  dive  plane  angle 

<A  rudder>  =  REAL  ;  Ordered  rudder  plane  angle 

<rangel>  =  REAL  ;  Forward  transducer  range 

<range2>  =  REAL  ;  Left  transducer  range 

<range3>  =  REAL  ;  Right  transducer  range 

<range4>  =  REAL  ;  Depth  transducer  range 

<speed>  =  REAL  ;  output  from  paddlewheel  sensor 
<wildcard>+  =  REAL  ;  available  for  user-defined  data  logging 


Figure  4.3 


NPS  AUV  telemetry  replay  data  file  format  specification 


D.  SOFTWARE  PROCESS  SPECIFICATIONS 

Numerous  mutually-dcpendent  processes  are  necessary  for  an  AUV  to  have 
adequate  capabilities  to  perform  independent  missions.  Software  module  specifications 
need  to  be  standardized  and  clearly  defined  in  a  large  multi-year  multi-programmer 
project  such  as  the  NPS  AUV.  Figure  4.4  is  a  sample  format  that  can  be  used  to 
record  individual  process  specifications  so  that  NPS  AUV  programming  group 
members  are  able  to  provide  proper  process  inputs  and  outputs.  A  group  commitment 
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to  defining,  following  and  upda.ting  fpnnal  specifications  is  essential  for  reliable 
process  interaction.  Failure  to  follow  formal  specifications  will  inevitably  lead  to  loss 
of  reliability i  mission  failure  and  probable  vehicle  loss. 

E.  CONCLUSIONS 

Many  software  modules  Have  been  written  for  eventual  use  by  the  NPS  AUV  but 
very  few  have  been  implemented  and  used  on  the  vehicle.  This  chapter  has  presented 
design  specifications  and  basic  software  engineering  considerations  for  the  NPS  AUV 
that  are  essential  for  compatible  operation  of  the  NPS  AUV  Integrated  Simulator. 
Numerous  additional  requirements  have  yet  to  be  formally  evaluated.  A  cohesive 
software  engineering  approach  is  needed  to  formally  define  system  software 
architecture,  process  specifications,  process  data  requirements,  standardization  of 
process  outputs  and  software  version  control.  Failure  to  address  these  requirements 
will  handicap  group  research  and  prevent  full  implementation  of  NPS  AUV  mission 
software. 
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NPS  AUV  Software  Process  Name 


Process  Short  Description 


Functional  Specifications 


Inputs 


Outputs 


Timing  and  Periodicity  Constraints 


Software  Process  Interfaces 


References  and  Additional  Information 


Individuals  Assigned 


Figure  4.4  NPS  AUV  software  process  summary  sheet 
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V.  NPS  AUV  INTEGRATED  SIMULATOR  DATA  NETWORK 

A.  INTEGRATED  SIMULATOR  DATA  NETWORK  OBJECTIVES 

The  purpose  of  the  NPS  AUV  Integrated  Simulator  data  network  is  to  connect 
all  workstation  and  personal  computer  nodes  with  the  NPS  AUV  and  laboratory  AUV 
microprocessors.  This  has  been  accomplished  by  connecting  several  local  area 
networks  with  a  wide  area  network  and  then  adding  modems  for  additional  long 
distance  connectivity.  This  chapter  describes  how  network  protocols  were  chosen  and 
network  connections  are  implemented  to  support  the  NPS  AUV  Integrated  Simulator. 

B.  NETWORK  CONNECTIVITY  REQUIREMENTS  AND  ETHERNET 

The  NPS  AUV  project  has  already  supported  thesis  work  for  dozens  of  graduate 
students  and  will  continue  to  be  active  for  the  foreseeable  future.  The  NPS  AUV 
Integrated  Simulator  data  network  is  designed  to  maximize  accessibility  to  NPS  AUV 
hardware  and  software.  Network  connectivity  objectives  must  address  many 
requirements  including  compatibility,  flexibility,  cost-effectiveness,  portability, 
maintainability,  future  growth  potential  and  ability  to  upgrade.  These  criteria  are  all 
dependent  upon  the  type  of  network  protocol  chosen. 

Ethernet  is  the  primary  local  area  network  protocol  used  at  NPS.  Ethernet  is 
used  for  the  Computer  Science  Department  network  in  Spanagel  Hall  and  for  the  NPS 
Campus-Wide  Network.  Ethernet  is  a  broadcast  bus  network  with  a  data  transfer 
capacity  of  10  M  bps.  It  is  a  broadcast  network  because  all  transceivers  receive  every 
transmission  and  a  bus  network  because  all  nodes  are  joined  by  a  common 
communications  channel.  Bridge  boxes  are  used  to  connect  various  departmental  local 
area  networks  to  the  Campus-Wide  Network.  These  bridges  screen  unnecessary  traffic 
from  transmission  to  adjacent  networks,  and  also  pass  copied  packets  of  interest 
without  reproduction  of  noise  or  packet  collisions. 
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Protocols  describe  methods  of  passing  messages,  formatting  data  arid  handling 
error  conditions.  TCP/BP  (Transmission  Control  Protocol/Internet  Protocol)  is.  the 
official  name  of  the  Internet  Protocol  Suite  used  by  Ethernet.  Ethernet  and  TCP/IP 
include  the  protocols  used  to  pass  files  or  data  packets  between  nodes.  FTP  (File 
Transfer  Protocol)  is  a  specific  means  of  file  transfer  that  can  be  used  under  both  Unix 
and  OS-9  operating  systems  to  transfer  files  between  NPS  AUV  processes  and  the 
graphics  simulation  workstation; 

Individual  nodes  connected  to  Ethernet  have  unique  physical  addresses.  Because 
the  NPS  local  area  networks  are  also  connected  to  the  Internet,  added  nodes  receive 
independent  Internet  addresses.  The  Internet  is  a  national  network  connecting 
thousands  of  government,  corporate  and  university  computer  networks.  Internet 
includes  links  to  all  other  major  national  and  international  networks.  Utilizing 
off-the-shelf  Ethernet  technology  for  the  NPS  AUV  Integrated  Simulator  is  desirable 
since  it  allows  complete  compatibility  with  existing  NPS  networks  as  well  as 
accessibility  via  the  Internet. 

Bandwidth  is  a  critical  consideration  when  choosing  a  network  protocol.  Large 
amounts  of  data  are  required  for  NPS  AUV  Integrated  Simulator  playback  of  vehicle 
performance,  and  numerous  message  outputs  from  many  software  processes  may  be 
required  to  adequately  evaluate  AUV  mission  execution.  The  network  and  protocol 
chosen  must  be  capable  of  transmitting  data  at  a  rate  adequate  to  support  real-time 
playback  and  simulation.  The  data  transfer  mechanisms  defined  in  Chapters  III  and  IV 
contribute  to  efficient  communication  by  concisely  representing  comprehensive  vehicle 
state  information.  Current  experience  with  interactive  simulation  programs  (Ong  90) 
(Jurewicz  90)  have  shown  that,  under  normal  network  usage  rates,  the  Ethernet 
10  M  bps  channel  capacity  is  adequate  to  meet  NPS  AUV  Integrated  Simulator  data 
transfer  bandwidth  requirements.  Ethernet  is  therefore  a  good  choice  for  the 
NPS  AUV  Integrated  Simulator  Data  Network  since  most  network  connections  were 
originally  in  place  and  network  bandwidth  is  adequate  to  support  simulation 
requirements. 
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The  NPS  AUV  project  may  eventually  need  a  special  long  distance  connection 
to  a  proposed  remote  AUV  laboratory  in  Building  230  at  the  NPS  Golf  Course. 
Building  230  is  approximately  1.3  miles  from  Spanagel  Hall,  a  distance  that  falls 
within  the  nominal  1.7  mile  maximum  range  for  ah  Ethernet  network.  However 
stringing  Ethernet  cable  and  repeaters  along  public  roads  to  the  golf  course  is 
obviously  not  feasible.  Several  options  are  available  to  maintain  NPS  AUV  Data 
Network  connectivity  with  a  remote  site  such  as  Building  230.  A  leased  high-speed 
phone  line  and  additional  interface  equipment  might  be  used  to  exchange  real-time  lab 
data  with  the  Campus-Wide  Network.  In  support  of  long-term  research  the 
Building  230  laboratory  can  be  made  an  independent  Internet  node  that  includes  both 
the  NPS  AUV  and  a  local  graphics  workstation.  Finally,  a  standard  phone  line  and  a 
modem  connection  can  be  used  for  file  transfer  to  the  Computer  Science  Department 
network  as  is  already  possible  during  pool  testing.  It  is  interesting  to  consider  that 
standard  telephone  connections  can  also  be  used  to  communicate  with  the  NPS  AUV 
at  other  remote  locations  such  as  the  Monterey  Bay  Aquarium,  Navy  laboratories  or  a 
coastal  launch  site. 

Other  network  configurations  and  data  link  methods  were  also  considered  but 
deemed  inappropriate.  Direct  customized  serial  or  parallel  port  interfaces  between  an 
Iris  graphics  workstation  and  a  laboratory  AUV  Gespac  are  possible  but  implementing 
such  ports  is  expensive,  time-consuming  and  incompatible  with  other  networked 
computer  systems  at  NPS.  Such  a  customized  communication  setup  also  risks 
becoming  obsolete  if  any  of  the  current  vehicle  hardware  is  changed  or  upgraded. 
Direct  port-to-port  communication  links  also  prevent  linking  multiple  independent 
software  processes  running  on  an  AUV  microprocessor  and  the  computer  science 
network.  Non-Ethernet  network  topologies  such  as  token-ring  or  Fiber  Distributed 
Data  Interface  (FDDI)  were  rejected  for  similar  reasons. 

A  direct  Ethernet  connection  inside  the  actual  NPS  AUV  is  operationally 
undesirable  due  to  space  and  power  consumption  requirements.  However  addition  of 
an  optional  Ethernet  card  to  be  used  when  the  NPS  AUV  is  in  the  laboratory  provides 
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a  high  data  transfer  channel  bandwidth  (10  M  bps)  and  improved  pseudo-mission 
testing  response.  Actual  AUV  system  reconfiguration  to  include  the  Ethernet  card  is 
not  a  problem  since  that  is  the  normal  configuration  of  the  standalone  laboratory  AUV 
microprocessor. 

C.  NETWORK  HARDWARE  REQUIREMENTS 

Numerous  components  are  connected  to  form  the  NPS  AUV  Integrated 
Simulator  data  network,  shown  in  Figuie  5.1.  Most  network  connections  were  already 
available  at  the  start  of  this  thesis.  Notable  construction  work  performed  included 
assembly  of  the  laboratory  AUV  Gespac  and  support  equipment  (node  auvsiml )  as 
well  as  the  support  laptop  (node  auvsiml )  and  the  support  PC  (unconnected  node 
auvsim3 ).  The  pertinent  portions  of  the  NPS  Computer  Science  Department  network 
are  shown  in  Figures  5.2  and  5.3,  and  the  NPS  Campus-Wide  Network  is  shown  in 
Figure  5.4. 

The  most  challenging  task  in  the  development  of  the  NPS  AUV  Integrated 
Simulator  data  network  was  to  build  a  laboratory  AUV.  An  additional  Gespac 
microprocessor  identical  to  the  NPS  AUV  Gespac  microprocessor  was  purchased  for 
simulation  use.  A  VME-bus  backplane  cage  containing  a  Gespac  microprocessor, 
input/output  channels  and  an  Ethernet  interface  card  was  installed  inside  a  cannibalized 
workstation  box.  A  second  backplane  cage  was  combined  with  the  first  to  provide 
extra  capacity  for  evaluating  additional  cards  prior  to  installation  on  the  actual 
NPS  AUV.  A  pair  of  monitor  and  keyboard  hookups  were  connected  to  communicate 
with  the  microprocessor.  The  laboratory  AUV  was  installed  adjacent  to  the  graphics 
simulation  workstations,  since  the  laboratory  Gespac  microprocessor  and  support 
terminals  need  to  be  directly  visible  from  the  simulation  workstations  for  efficient 
testing  and  troubleshooting.  This  also  made  the  lab  AUV  accessible  to  the  local  area 
network  ethemet  cable.  The  support  PC  includes  an  OS-9  "C"  language 
cross-compiler  which  produces  GESPAC  68020/68030  object  code. 
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Figure  5.2  NPS  Computer  Science  Department  Network  portion  of 
NPS  AUV  Integrated  Simulator  data  network  (part  1) 
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Figure  5.3  NPS  Computer  Science  Department  Network  portion  of 
NPS  AUV  Integrated  Simulator  data  network  (part  2) 


The  laboratory  Gespac  microprocessor  is  configured  almost  identically  to  the 
NPS  AUV  Gespac  in  order  to  allow  realistic  and  thorough  simulation.  The  laboratory 
Gespac  differs  from  the  actual  NPS  AUV  Gespac  by  having  an  Ethernet  card 
connection  to  the  Computer  Science  Department  network  and  serial  port  connections 
to  a  VT-220  terminal  and  a  support  PC.  Care  has  been  taken  to  preserve  physical  and 
functional  equivalence  between  laboratory  and  NPS  AUV  Gespac  microprocessors. 

The  current  laboratory  AUV  card  cage  configurations  is  shown  in  Figure  5.5. 

The  Gespac  68020/68030  microprocessor  running  OS-9  requires  Erasable 
Programmable  Read-Only  Memory  (EPROM)  modifications  for  permanent 
configuration  changes.  However  any  desired  operation  can  be  performed  without 
performing  tedious  a  EPROM  change  because  system  drivers  can  be  added  as  needed 
from  the  OS-9  command  line.  The  current  network  configuration  of  the  laboratory 
Gespac  microprocessor  will  allow  development  and  implementation  of  communications 
sockets  to  pass  data  packets  for  real-time  simulator  display. 
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Prior  to  implementation  of  the  NPS  AUV  Integrated  Simulator  data  network,  the 
Mechanical  Engineering  Department  network  was  not  connected  to  any  external 
network.  A  special  interface  board  was  purchased  and  installed  during  1991  that 
linked  their  DECNET-based  system  to  the  Campus-Wide  Network  backbone  cable 
running  through  Halligan  Hall.  Another  Iris  graphics  workstation  is  also  on  this  local 
area  network,  permitting  local  execution  of  graphic  simulation  as  part  of  the  overall 
NPS  AUV  Integrated  Simulator.  Another  laboratory  Gespac  is  also  available  in 
Halligan  Hall.  Each  of  these  nodes  can  now  be  networked  to  take  advantage  of  the 
many  benefits  of  integrated  simulation. 

D.  OPERATING  SYSTEM  INTERFACES 

The  Computer  Science  Department  network  uses  the  Unix  operating  system. 
Although  different  versions  of  Unix  can  coexist  on  subnetworks  connected  to  the 
departmental  network,  each  is  compatibly  connected  using  Ethernet  and  Internet 
TCP/IP  protocols.  Further  capability  can  be  provided  by  socket  interface  software  that 
allows  application  programs  to  directly  access  communication  protocols  via  the 
operating  system.  Sockets  for  interprocess  and  intermachine  communication  have  been 
written  and  implemented  under  Unix  (Barrow  88)  (Ong  90).  The  Gespac  OS-9 
operating  system  also  supports  Ethernet  and  TCP/IP  protocols  but  further  work  is 
needed  to  implement  software  sockets  under  the  Gespac  OS-9  operating  system. 

Complex  simulator  testing  of  the  NPS  AUV  may  someday  be  based  on  real-time 
data  transfer  between  AUV  software  processes  and  the  integrated  simulator. 

NPS  AUV  software  processes  have  two  possible  operating  environments:  simulated 
developmental  testing  under  the  Unix  operating  system  or  operational  execution  under 
the  OS-9  operating  system  on  a  Gespac  microprocessor.  Although  file  transfer  is  the 
only  data  passing  mechanism  currently  implemented,  it  is  desirable  to  develop 
real-time  testing  capabilities  in  both  operating  system  environments  by  implementing 
sockets  to  pass  data  packets.  If  data  transfer  between  processes  succeeds 
independently  of  the  NPS  AUV  under  the  developmental  Unix  environment,  it  is 
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reasonable  to  expect  that  the  same  software  processes  will  still  communicate  properly 
when  ported  to  the  NPS  AUV’s  resident  OS-9  operating  system.,  Careful 
implementation  of  data  packet  passing  and  network  sockets  will  ensure  proper 
portability  of  processes  that  employ  sockets. 

Use  of  ASCH  text  files  for  mission  logging  makes  encryption  a  simple  matter  if 
increased  security  becomes  necessary  due  to  mission  requirements.  Encryption  can  be 
performed  prior  to  file  transfer  or  during  high-level  object  file  message  logging.  Any 
encryption  routines  used  need  to  comply  with  the  DoD  Encryption  Standard  (DES)  of 
higher  requirement.  Encrypted  output  files  need  to  remain  in  ASCII  text  format  for 
complete  communications  protocol  compatibility. 

E.  CONCLUSIONS 

Creating  an  NPS  AUV  Integrated  Simulator  Data  Network  by  linking  NPS  AUV 
hardware  with  graphics  workstations  will  provide  an  exceptional  tool  for  distributed 
research,  real-time  simulation  and  end-to-end  AUV  system  testing.  Ethernet  is  the 
appropriate  local  area  network  technology  to  use  in  order  to  ensure  complete 
compatibility  with  existing  NPS  networks.  Ethernet  and  Internet  connectivity  ensures 
that  the  NPS  AUV  Integrated  Simulator  data  network  completely  supports  all 
NPS  AUV  research,  development  and  testing.  Further  work  remains  to  implement 
TCP/IP  protocol  and  software  socket  compatibility  under  the  OS-9  operating  system 
running  on  the  Gespac  68030  architecture. 

Connection  of  the  NPS  AUV  Integrated  Simulator  data  network  allows 
preliminary  evaluation  of  NPS  AUV  software  changes  to  be  performed  prior  to 
in-water  testing.  Concurrent  playback  and  data  verification  is  now  possible  during 
pool  testing  by  transferring  telemetry  replay  files  over  telephone  lines  during 
NPS  AUV  test  runs.  Immediate  problem  diagnosis  allows  immediate  correction  and 
repetition  of  test  runs  that  result  in  corrupted,  faulty  or  unusual  data. 

Additional  work  needs  to  be  done  on  the  NPS  AUV  Integrated  Simulator  data 
network  when  modeling  or  simulating  missing  NPS  AUV  hardware  in  the  graphics 
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lab.  It  is  likely  that  some  world  models  will  not  fit  on  the  operational  Gespac 
requiring  data  packet  communication  with  an  offline  world  model.  The 
laboratory  AUV  is  an  excellent  testbed  for  evaluating  new  potential  NPS  AUV 
physical  components.  Finally,  the  data  network  has  the  growth  potential  to  support  the 
addition  of  analog  NPS  AUV  hardware  components,  allowing  the  NPS  AUV 
Integrated  Simulator  to  become  a  true  digital  and  analog  hybrid  simulator. 
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VI.  AUTONOMOUS  SONAR  CLASSIFICATION  USING  EXPERT  SYSTEMS 
A.  ABSTRACT 

An  expert  system  can  process  active  sonar  returns,  perform  geometric  analysis 
and  autonomously  classify  detected  underwater  objects.  Autonomous  classification  of 
objects  is  an  essential  requirement  for  independent  operation  by  autonomous 
underwater  vehicles  (AUVs).  Most  AUVs  are  only  capable  of  rudimentary  sensor 
analysis,  since  standard  approaches  to  evaluation  and  classification  of  sonar  data 
require  excessive  signal  processing  and  computational  power  to  be  practical.  This 
chapter  describes  how  to  develop  an  autonomous  sonar  classification  expert  system  for 
a  working  AUV. 

A  fundamental  approach  is  presented  for  applying  geometric  reasoning  and 
expert  system  heuristics  to  sonar  classification.  Preliminary  sonar  processing  is 
performed  using  parametric  regression  line  fitting.  A  polyhedron-building  algorithm 
correlates  the  parametric  regression  line  segments  into  geometric  objects.  After 
quantifying  geometric  object  attributes,  objects  are  classified  using  rule-based 
evaluation  of  quantitative  and  qualitative  attributes  combined  with  sonar  classification 
heuristics. 

A  summary  of  expert  systems  describes  their  salient  features  pertinent  to 
autonomous  sonar  classification  systems.  The  expert  system  paradigm,  knowledge 
representation,  reasoning  using  facts  and  rules,  rule  sets,  control  of  execution  flow  and 
expert  system  development  are  outlined.  Expert  system  self-diagnosis  and 
self-correction  also  discussed. 

Implementation  was  performed  using  the  "C"  Language  Integrated  Production 
System  (CLIPS)  expert  system  shell.  Real-time  graphic  simulation  and  scientific 
visualization  are  employed  to  evaluate  results.  Experimental  sonar  classification 
results  are  presented  using  actual  mission  data  from  the  Naval  Postgraduate  School 
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(NPS)  AUV.  Successful  classifications  of  walls. arid’ a  mine-like  object  are 
demonstrated. 

B.  INTRODUCTION 

Intelligent  vehicles  will  play  a  major  role  in  future  underwater  missions.  A 
critical  requirement  for  independent  behavior  by  such  vehicles  is  autonomous  analysis 
of  complex  and  variable  ocean  environments.  This  is  a  notoriously  difficult  task,  even 
when  human  operators  use  sophisticated  sensors  and  powerful  processors. 

Although  much  work  has  been  done  in  vision  processing  for  mobile  robots, 
additional  research  has  been  needed  on  interpretation  of  observed  scenes  and  terrain 
(Hebert  88).  Numerous  approaches  to  the  general  object-recognition  problem  are 
presented  in  (Besl  85).  Both  of  these  references  can  be  found  in  (Iyengar  91),  an 
essential  collection  of  surveys,  tutorials  and  fundamental  research  papers  regarding 
mobile  robot  sensor  perception,  mapping  and  navigation.  Other  references  included  in 
(Iyengar  91)  are  (Luo  89)  and  (Moravec  83). 

Independent  and  meaningful  intetpretation  of  sensor  data  is  a  principal 
prerequisite  for  accomplishing  high-level  AUV  missions  and  behaviors.  A  number  of 
universities  and  laboratories  are  conducting  autonomous  underwater  vehicle  (AUV) 
research  and  development  that  involves  a  wide  variety  of  sensor  types  and  sensor 
interpretation  methods.  The  Defense  Advanced  Research  Projects  Agency  (DARPA) 
Unmanned  Undersea  Vehicle  (UUV)  uses  sidescan  sonar  and  neural  network 
classification  for  underwater  mine  detection  (Pappas  91).  Woods  Hole  Oceanographic 
Institution  has  used  sidescan  sonar,  stochastic  backprojection  and  a  variety  of  vision 
processing  techniques  and  sea  floor  shape  information  to  create  three-dimensional 
bottom  images  (Stewart  89).  The  University  of  New  Hampshire  Experimental 
Autonomous  Vehicle  (EAVE)  HI  uses  depth  profiling,  acoustic  long  baseline 
navigation  and  comparison  with  a  world  model  to  detect  bottom  objects  (Blidberg  90). 
Numerous  other  examples  of  sensor  data  interpretation  exist.  In  contrast  to  most 
methods,  this  sonar  classification  system  uses  parametric  regression,  geometric  analysis 
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and  expert  system  heuristics  to  create  classifiable  object  types.  An  advantage  of  this 
method  is  that  progressively  higher  levels  of  object  abstraction  are  possible. 

C.  OVERVIEW 

The  objective  of  this  chapter  is  to  present  a  method  for  autonomous 
classification  of  underwater  objects.  This  is  achieved  using  geometric  sonar  analysis 
techniques  and  an  expert  system  for  heuristic  reasoning. 

This  research  effectively  demonstrates  that  geometric  analysis  can  be  combined 
with  an  expert  system  to  process,  analyze  and  classify  active  sonar  range  and  bearing 
data  in  support  of  AUV  operations.  Figure  6.1  shows  how  low-level  sonar  data  is 
processed  to  produce  increasingly  complex  geometric  objects  and  high-level 
classification  outputs. 

Geometric  analysis  can  distill  large  amounts  of  sonar  data  into  useful 
information  that  can  be  used  to  make  logical  and  informed  decisions.  The  primary 
difficulty  in  geometric  sonar  analysis  is  that  active  sonar  signal  returns  are  inherently 
noisy  and  unconnected.  Parametric  regression  is  a  robust  method  of  least-squares  line 
fitting  that  permits  precise  geometric  analysis  of  range  and  bearing  data  (Floyd  91). 
Generated  regression  lines  are  provided  to  a  polyhedron-building  algorithm  to  create 
geometric  objects.  Geometric  object  attributes  can  then  be  compared  to  known  object 
types  through  the  rule-based  pattern-matching  capabilities  of  an  expert  system, 
resulting  in  object  classification. 

The  possible  types  of  object  classes  to  be  detected  are  typically  limited  in 
number  and  somewhat  predictable  given  a  priori  knowledge  of  the  underwater 
environment.  Geographic  objects  to  be  detected  and  classified  include  the  ocean 
bottom,  sea  mountains,  valleys,  rock  outcroppings  and  walls.  Biological  objects 
include  fish,  kelp,  scuba  divers  and  large  animals  such  as  dolphin  or  whales. 
Man-made  objects  include  ships,  submarines,  torpedoes,  mines,  nets,  pipes  and  cables. 
These  object  classes  of  interest  are  listed  in  Table  I.  The  relatively  small  number  of 
underwater  objects  of  interest  simplifies  sonar  classification  criteria.  Primary  expert 
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Sonar  Range  and  Bearing  Inputs 

Extract  Line  Segments 
using  Parametric  Regression 

■  ■  m 

l.i  -.vy 

Build  Polyhedron 
from  Line  Segments 

m 

Quantify  Polyhedron  Attributes 

Pattern-match  Classification 

Classified  Object  Output 

Figure  6.1  Autonomous  sonar  classification  process  diagram 


system  outputs  are  location,  size,  and  classification  of  all  sonar  contacts. 

Expert  systems  are  an  established  methodology  that  can  effectively  and  clearly 
represent  specialized  human  knowledge  using  algorithms  and  heuristic  rules.  Typically 
the  functions  performed  by  an  expert  system  might  otherwise  require  human  action  by 
a  knowledgeable  expert.  The  expert  system  approach  is  applicable  to  a  wide  variety 
of  complex  problems,  even  when  no  single  expert  understands  all  aspects  of  a 
particular  problem  domain. 
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Table  VIJ  EXAMPLE  UNDERWATER  OBJECT  CLASSIFICATION 
TYPES 


Geographic 

Biological 

Manmade 

Ocean  bottom 

Fish 

Ship 

Sea  mountain 

Kelp 

Submarine 

Valley 

Scuba  diver 

Mine 

Rock  outcropping 

Dolphin 

Torpedo 

Wall 

Whale 

Net 

Sea  surface 

Shark 

Pipe  or  cable 

Unknown 

The  use  of  real  world  data  is  important  for  development  and  verification  of  a 
sonar  classification  expert  system.  Naval  Postgraduate  School  (NPS)  students  and 
faculty  have  designed  and  built  a  working  AUV  that  can  be  used  to  provide  a  variety 
of  classifiable  sonar  data.  Successful  examples  of  expert  system  classifications  using 
NPS  AUV  sonar  data  are  described  in  detail. 

The  expert  system  approach  also  appears  to  be  usable  for  sensor  fusion  using  a 
wide  variety  of  sonar  types  as  well  as  non-acoustic  sensors  such  as  laser  rangefinders 
and  video.  Many  excitmj;  future  applications  are  possible  using  expert  system 
methods. 

D.  GEOMETRIC  ANALYSIS  OF  SONAR  DATA 
1.  General  Characteristics  of  Active  Sonar  Data 

Outputs  common  to  practically  all  active  sonars  are  range  and  bearing  from 
the  sonar  transducer  to  a  contact,  if  any  is  detected.  Posture  of  an  underwater  vehicle 
includes  a  three-dimensional  position  coordinate,  as  well  as  vehicle  attitude  consisting 
of  roll,  elevation  and  azimuth  orientations.  The  relative  position  of  each  sonar  return 
is  combined  with  vehicle  posture  using  vector  addition  to  yield  a  precise 
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three-dimensional  coordinate.  In  this  chapter  the  term  "sonar  data"  refers  to 
simultaneous  sonar  range  and  bearing  data  returned  from  an  active  sonar  transmission. 

2.  Geometric  Primitives  and  Object  Attribute  Definitions 

Sonar  data  can  be  analyzed  to  produce  geometric  forms  such  as  points, 
lines  or  polyhedra.  Precise  definitions  of  geometric  primitives  and  object  attributes  are 
necessary  for  predictable  and  repeatable  sonar  classifier  performance.  It  is  important 
that  the  theoretical  basis  of  a  sonar  classification  expert  system  be  both  mathematically 
rigorous  and  as  general  as  possible  in  order  to  allow  increasingly  sophisticated  analysis 
of  data.  A  formal  geometry-based  approach  also  permits  expert  system  compatibility 
with  a  wide  variety  of  sonar  types. 

The  geometric  primitives  considered  by  this  expert  system  are  point,  line 
segment,  polyhedron  and  cylindrical  polyhedron  (i.e.  a  three-dimensional  polyhedron 
that  extends  vertically  up  and  down  from  a  planar  polygon  perimeter).  Object 
attributes  include  centroid  position,  depth,  length,  width,  height,  perimeter, 
cross-sectional  area,  thinness,  and  volume.  Indirect  attributes  such  as  positional 
accuracy,  confidence  factor,  inferred  edges  and  hidden  edges  are  also  evaluated. 

Additional  geometric  primitives  and  object  attributes  can  be  defined  as 
necessary  to  utilize  the  more  sophisticated  data  available  from  sector  scanning, 
two-dimensional  swath  or  three-dimensional  multi-beam  sonars.  Similar  approaches 
using  curved  shapes  such  as  circles,  ellipses  or  conics  (Moravec  83)  are  also 
compatible. 

3.  Extracting  Line  Segments  using  Parametric  Regression 

Linear  relationships  described  by  sets  of  discrete  data  are  typically  found 
using  standard  linear  regression  analysis,  commonly  known  as  least-squares  fit.  This 
method  is  widely  used  but  has  a  significant  limitation  in  that  regression  calculations  on 
(x,y)  coordinate  points  parallel  to  the  y-axis  result  in  divide-by-zero  singularities  for 
slope  and  mathematically  undefined  regression  results.  Since  typical  unconstrained 
sonar  data  may  lie  along  any  three-dimensional  orientation,  a  different  method  is 
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needed  for  autonomous  fitting  of  best-approximation  line  segments  to  a  series  of 
discrete  sonar  returns. 

The  parametric  regression  method  utilizes  a  polar  coordinate  derivation  of 
linear  regression  analysis  to  provide  a  robust  and  accurate  least-squares  fit  of  line 
segments  to  sequences  of  data  points.  This  method  has  been  developed  in  detail  and 
is  particularly  well  suited  for  geometric  analysis  of  real-world  sonar  data 
(Kanayama  89)  (Kanayama  90)  (Floyd,  Kanayama,  Magrino  91)  (Floyd  91). 
Associated  with  each  regression  line  segment  is  an  elliptical  thinness  term  that  can  be 
used  as  a  metric  for  line  segment  accuracy  and  data  variance.  Figure  6.2  shows  a 
typical  parametric  regression  line  segment  fit  to  a  set  of  sonar  returns. 


Figure  6.2  Typical  parametric  regression  line  fit 


A  further  significant  benefit  of  parametric  regression  analysis  is  that  it  is  a 
sequential  algorithm  which  provides  immediate  incremental  improvements  upon  receipt 
of  each  individual  data  point.  The  sequential  nature  of  this  algorithm  makes  it  highly 
suited  for  real-time  operations  that  must  meet  immediate  response  requirements. 
Real-time  vehicles  cannot  afford  to  wait  for  intermittently  time-consuming  sonar 
analysis  when  excessive  delay  might  jeopardize  navigational  safety. 

4.  Building  a  Polyhedron  from  Line  Segments 

Parametric  regression  provides  linear  one-dimensional  geometric  primitives. 
However  line  segments  by  themselves  are  insufficient  for  thorough  two-dimensional 
spatial  reasoning  or  object  classification.  A  polyhedron-building  algorithm  is 
presented  here  as  a  means  of  constructing  two-dimensional  geometric  objects  from  a 
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sequence  of  regression  line  segments.  In  this  context  the  polyhedron-building 
algorithm  is  a  logical  extension  to  the  parametric  regression  algorithm. 

One  important  assumption  used  when  building  polyhedra  is  that  underwater 
contacts  of  interest  have  predominantly  convex  shapes,  i.e.  they  contain  no  large 
concave  depressions  or  cavities.  This  assumption  permits  clear  delineation  of 
independent  object  boundaries.  Analysis  of  an  actual  concave  object  results  in  the 
definition  of  adjacent  convex  objects.  Higher-level  analysis  at  the  heuristic  level  can 
be  used  to  clump  adjacent  objects  if  needed. 

Note  that  the  orientation  of  vehicle  sonar  relative  to  detected  objects  is  a 
critical  consideration  in  the  polyhedron  building  algorithm,  since  spatial  relationships 
are  equally  dependent  on  sensor  perspective  and  actual  object  shape. 

Polyhedron  building  begins  with  a  single  line  segment  produced  by 
parametric  regression  analysis  of  continuous  sonar  data.  Each  following  segment  from 
regression  analysis  on  the  same  sensor  is  compared  to  the  previous  segment.  If  the 
follow-on  segment  meets  proximity  and  orientation  criteria,  then  it  is  considered  to  be 
another  part  of  the  same  geometric  object.  This  segment  comparison  process  is 
repeated  until  proximity  or  orientation  criteria  fail,  at  which  time  the  previous 
geometric  object  is  complete  and  the  follow-on  segment  becomes  the  beginning 
segment  of  a  new  geometric  object. 

Proximity  is  measured  between  the  end  point  of  the  most  recently 
correlated  line  segment  and  the  start  point  of  the  next  segment  to  be  considered.  The 
proximity  criterion  is  typically  small  and  restrictive  (e.g.  less  than  1  foot)  in  order  to 
permit  discrimination  between  adjacent  objects.  The  proximity  criterion  must  be  met 
prior  to  comparing  relative  orientation  for  geometric  object  extension. 

Orientation  comparisons  are  made  to  determine  whether  adjacent  segments 
are  colinear,  convex  or  concave.  The  colinear  test  allows  a  reasonable  error  bound 
(e.g.  ±10°)  in  order  to  account  for  sonar  noise  and  line-fitting  approximations. 

Colinear  segments  are  acceptable  for  geometric  object  extension  (Figure  6.3). 
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The  convex  test  measures  whether  the  follow-on  segment  direction  points 
farther  away  from  the  sensor’s  perspective  than  the  previous  segment.  Convex 
segments  are  also  acceptable  for  geometric  object  extension  (Figure  6.4). 


Figure  6.4  Examples  of  convex  regression  line  segments 


The  concave  test  measures  whether  the  follow-on  segment  direction  points 
closer  towards  the  sensor’s  perspective  than  the  previous  segment,  in  effect  defining 
the  boundaries  of  a  hole.  Concave  line  segment  relative  orientations  indicate  a  break 
between  separate  convex  geometric  objects  (Figure  6.5).  The  follow-on  segment  is 
used  to  start  a  new  polyhedron. 

Inferred  edges  are  presumed  to  exist  between  each  pair  of  the  sequential 
detected  edges  that  make  up  a  polyhedron.  A  single  hidden  edge  is  presumed  to  exist 
between  the  start  point  and  end  point  of  a  particular  object.  The  classifier  must 
recognize,  however,  that  such  hidden  edges  may  be  completely  inaccurate  since  the 
actual  hidden  sides  of  the  object  were  obscured  from  the  sonar. 
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In  summary,  the  polyhedron-building  algorithm  correlates  regression  line 
segments  into  two-dimensional  polyhedral  objects.  This  method  enables  the 
application  of  computational  geometry  techniques  to  analyze  large  volumes  of  discrete 
range  and  bearing  data.  Figure  6.6  illustrates  the  polyhedron-building  algorithm. 

5.  Quantifying  Polyhedron  Attributes 

The  attributes  that  are  used  to  classify  objects  need  to  be  precisely  defined 
and  calculated,  wherever  possible.  For  example,  attributes  such  as  depth,  length,  width 
and  height  are  directly  measurable  using  calculated  sonar  positions.  Object  perimeter 
can  be  determined  by  first  summing  the  lengths  of  all  correlated  line  segments,  and 
then  adding  the  lengths  of  all  inferred  and  hidden  edges  that  are  presumed  to  exist 
between  detected  edges.  Figure  6.7  shows  how  the  start  point,  regression  line 
segments,  inferred  edges  and  hidden  edge  that  make  up  a  polyhedron  cross-section 
define  a  series  of  triangular  areas. 

Area  of  a  single  triangle  is  given  by  Equation  (6.1). 

Areau  =  ±  [  -  (X ]  («•!) 


Polyhedron  cross-sectional  area  is  determined  by  summing  the  area  of  these 
triangles,  given  by  Equation  (6.2). 
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Figure  6. 7  Summing  triangle  areas  to  determine  polyhedron  cross-sectional 

area 
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Centroid  position  for  a  triangle  is  calculated  using  Equation  (6.3). 


Triangle  _  /y  v  >  _ 
Centroid  ~  »  rc)  - 
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(6.3) 


Centroid  position  for  the  polyhedron  cross-section  is  precisely  determined 
by  taking  the  weighted  average  of  each  of  the  triangle  centroids,  given  by 
Equation  (6.4). 
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Polyhedron  cross-section  thinness  is  defined  as  the  ratio  of  polyhedron  area 
to  the  square  of  polyhedron  perimeter,  given  by  Equation  (6.5). 
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Polyhedron 
Cross-section  = 
Thinness 


Polyhedron  Area 
C Polyhedron  Perimeter )2 


(6.5) 


If  object  height  is  needed  and  has  not  been  directly  measured,  it  can  be 
estimated  using  heuristic  rules  based  on  object  depth,  bottom  depth  or  independent 
object  classification.  Object  volume  is  the  product  of  cross-sectional  area  and 
measured  or  estimated  object  height. 

Indirect  attributes  such  as  positional  accuracy,  confidence  factor,  inferred 
edges  and  hidden  edges  are  also  evaluated.  Point  positional  accuracy  is  derived  by 
combining  current  vehicle  positional  accuracy  estimate  with  sonar  accuracy  or  sonar 
beamwidth  at  the  range  to  the  object.  Confidence  factor  can  be  defined  independently 
of  positional  accuracy  as  a  measure  of  how  well  the  object  matches  a  classification 
rule.  Hidden  edge  length  is  a  measure  of  what  is  not  known  about  the  object. 

Defining  initial  classification  confidence  factor  as  the  ratio  between  hidden  edge  length 
and  detected  perimeter  further  indicates  how  much  of  the  contact  has  actually  been 
evaluated.  Hidden  edge  metrics  can  be  used  to  indicate  whether  further  sonar 
investigation  of  the  contact  is  desirable.  Figure  6.8  shows  detected  edges,  inferred 
edges  and  hidden  edge  relative  to  processed  sonar  returns,  and  how  these  geometric 
primitives  may  not  fully  reveal  all  features  of  a  contact. 

E.  EXPERT  SYSTEM  HEURISTICS  FOR  SONAR  CLASSIFICATION 

While  geometric  analysis  can  be  defined  with  mathematical  precision,  human 
knowledge  regarding  sonar  classification  is  less  rigorous  and  can  best  be  encoded  as 
expert  system  heuristics. 

1.  Classification  Heuristics  and  Attribute  Heuristics 

Sonar  classification  is  not  always  a  well  defined  problem.  For  example,  it 
is  possible  that  sonar  analysis  of  a  single  object  can  be  performed  from  different 
directions  and  lead  to  completely  different  classifications.  An  analogy  to  classifying 
objects  using  simple  range  and  bearing  sonars  is  attempting  to  identify  your 
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Figure  6.8  Polyhedron  detected  edges,  inferred  edges  and  hidden  edge  may 
not  fully  reveal  all  features  of  the  sonar  contact 


surroundings  while  looking  at  the  world  through  a  steerable  pinhole.  It  is  difficult! 
Consequently,  sonar  classification  criteria  are  often  ambiguous  and  difficult  to 
quantify,  even  when  using  formally  derived  geometric  primitives.  However,  the 
heuristic  approach  used  by  expert  systems  is  effective  in  many  types  of  inexact 
problems  and  enables  an  autonomous  system  to  obtain  excellent  sonar  classification 
results. 

Heuristics  can  be  used  for  evaluating  attributes  such  as  object  height  when 
information  is  incomplete.  Both  attribute  and  classification  heuristics  can  be  easily 
modified  in  understandable  ways  despite  the  ambiguities  of  sonar  analysis.  The 
intuitive  power  of  heuristics  combined  with  the  precision  of  geometric  analysis  gives 
sonar  classification  expert  systems  wide  applicability  and  adaptability. 

For  this  expert  system  approach,  classification  of  sonar  contacts  is 
performed  by  comparing  attributes  of  detected  objects  with  predetermined  attributes  of 
known  objects  of  interest.  Different  classification  criteria  are  necessary  and  desirable 
for  different  environments.  In  particular,  the  different  characteristics  of  deep  ocean 
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versus  shallow  water  versus  an  artificial  pool  will  constrain  the  possible  types  of 
objects  to  be  detected.  Knowledge  of  the  current  environment  can  be  extremely  useful 
when  determining  the  specialized  classification  rules  and  heuristic  criteria  to  be  used 
for  a  given  mission. 

Precise  classification  of  every  possible  object  type  may  not  be  necessary  for 
some  missions.  Resolution  of  an  ambiguous  classification  typically  requires  multiple 
sensor  looks,  costing  additional  time  and  energy.  Preliminary  classification  as  a 
potential  contact  of  interest  may  be  sufficient  to  justify  maneuvering  for  additional 
sensing  and  closer  investigation.  Conversely,  objects  deemed  to  be  of  no  interest 
require  no  further  investigation  by  the  vehicle. 

Size  can  be  the  primary  classification  attribute  for  most  underwater  objects 
of  interest.  However,  size  per  se  is  not  a  strictly  defined  term.  It  is  worth  mention 
that  significant  object  size  may  be  indicated  by  a  variety  of  attributes  including 
cross-sectional  area,  volume,  perimeter,  thinness  or  hidden  edge  length.  Any  or  all  of 
these  size-related  attributes  may  require  close  evaluation  in  order  to  properly 
discriminate  between  similarly  sized  sonar  targets  such  as  mines  and  rocks. 


2.  Pattern-match  Classification  Examples 

Examples  of  how  heuristic  rules  work  can  illustrate  how  a  sonar  expert 
system  can  classify  objects.  Two  examples  are  presented  here. 

Preliminary  wall  classification  is  possible  during  the  execution  of  the 
polyhedron-building  algorithm.  Walls  are  defined  as  any  flat  linear  surface  of 
non-trivial  length.  Polyhedra  being  built  can  be  considered  walls  as  long  as  each  of 
the  newly  added  regression  line  segments  meet  colinearity  and  proximity  criteria.  As 
soon  as  the  polyhedron-building  uiguuuuu  auus  a  new  line  &cgiiicnt  uuscu  on 
convexity  criteria,  the  polyhedron  being  built  can  be  immediately  reclassified  from 
wall  to  object  since  the  polyhedron  is  no  longer  linear. 

Once  a  polyhedron  has  been  built,  all  polyhedron  attributes  are 
automatically  calculated.  At  this  final  stage,  all  of  the  preliminary'  work  to 
quantitatively  determine  precise  geometric  objects  greatly  simplifies  object 
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classification.  For  instance,  a  polyhedron  might  be  classified  as  a  mine-like  object 
whenever  cross-sectional  area  is  between  10  and  100  square  feet  (Figure  6.9).  Other 
objects  can  be  classified  in  an  equally  straightforward  manner. 

Some  objects  should  not  be  uniquely  classified.  For  example, 
discrimination  between  a  scuba  diver  and  a  mine-like  object  may  be  difficult  A 
particular  strength  of  the  expert  system  approach  is  that  each  object  can  receive 
multiple  classifications  with  associated  confidence  factors  as  appropriate.  This  feature 
allows  high-level  reasoning  using  uncertainty,  rather  than  being  constrained  by  an 
arbitrary  and  potentially  incorrect  single  classification. 


(defrule  classify-mine-like-object 
;  if  this  left-hand  side  of  the  rule  is  found  to  be  true: 

?poly  <-  (Polyhedron  (status  COMPLETE) 
(classification  OBJECT) 

(start  ?startpolytime) 

(end  ?endpolytime) 

(area  ?area)) 


;  then  perform  this  right-hand  side  of  the  classification  rule: 

(if  (and  (>=  ?area  10.0)  (<=  ?area  100.0)) ;  area  criteria  test 

then  (modify  ?poly  (classification  MINE)) 

(printout  t  "The  polyhedron  at  times  "  ?startpolytime 

?endpolytime) 

(printout  t  "has  classification  MINE.") 


Figure  6.9  Classification  rule  for  a  mine-like  object 


What  was  originally  an  intractable  sonar  classification  problem  is  now 
much  simpler  and  understandable  at  the  highest  level  of  the  expert  system. 
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3.  Self-Diagnosis  and  Self-Correction 

An  additional  strength  of  the  expert  system  paradigm  is  that  rules  can  be 
written  to  evaluate  overall  system  performance,,  correcting  internal  vehicle  problems 
without  external  control.  Self-diagnosis  is  possible  when  expert  system  evaluation  of 
sensor  data  differs  from  a  priori  knowledge  of  the  real  world.  Such  differences  can  be 
automatically  fed  back  into  the  system  to  correct  the  offending  error.  As  an  example, 
gyro  error  and  gyro  drift  rate  can  be  diagnosed  and  quantified  when  a  deduced  wall 
orientation  does  not  match  known  geographic  data.  Updating  system  estimates  of  gyro 
error  and  gyro  drift  rate  result  in  an  immediate  improvement  in  sonar  accuracy  and 
positional  estimates. 

F.  EXPERT  SYSTEM  PARADIGM 

The  power  of  an  expert  system  is  essential  for  a  sonar  classifier  to  perform 
high-level  reasoning  using  qualitative  attribute  and  sonar  classification  heuristics.  This 
section  describes  the  salient  features  of  expert  systems  that  are  pertinent  to  the 
development  of  an  autonomous  sonar  classifier. 

1.  Expert  System  Characteristics 

An  expert  system  typically  includes  the  following  characteristics:  it 
simulates  human  reasoning  about  a  problem  domain,  it  uses  symbolic  knowledge 
representation  and  rules  of  thumb,  it  can  analyze  problems  using  heuristic  or 
approximate  methods  that  may  not  be  guaranteed  to  succeed,  and  it  deals  with 
complexity  that  normally  might  require  a  human  expert  (Jackson  90).  Expert  system 
development  differs  from  usual  software  engineering  approaches  in  that  rules  of  thumb 
can  be  developed  incrementally  to  solve  large  problems  that  do  not  necessarily  have  a 
clearly  defined  solution  methodology.  Complementary  rules  work  together  without 
explicit  supervision  to  discover  solutions,  if  any  exist. 
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2.  Knowledge  Representation  and  Reasoning  using  Facts,  Rules  and  an 
Inference  Engine 

Expert  systems  typically  use  facts  to  represent  knowledge  about  the  state  of 
the  problem  domain.  Facts  can  be  known  pror  to  execution  as  part  of  the  problem 
definition,  and  can  also  be  discovered  during  program  execution  as  new  data  becomes 
available  or  new  knowledge  is  deduced.  Rules  are  heuristic  representations  of  human 
reasoning  that  follow  the  condition-action  model.  If  a  rule  finds  certain  conditions  to 
be  true,  then  corresponding  actions  will  follow  and  the  rule  is  said  to  execute  or  "fire". 
The  inference  engine  is  the  mechanism  that  allows  all  of  the  rules  to  individually 
examine  the  fact  database  and  fire.  The  order  of  rule  precedence  and  firing  may  range 
from  random  sequencing  to  a  strictly  defined  execution  order. 

Strict  execution  order  is  typical  of  traditional  programming  paradigms  but  is 
usually  considered  to  be  an  undesirable  constraint  for  expert  systems.  Interestingly, 
random  firing  of  expert  system  rules  often  uncovers  solutions  to  problems  that  might 
otherwise  be  considered  unsolvable  using  a  strictly  defined  sequential  approach. 

3.  Rule  Sets  and  Control  of  Execution  Flow 

Given  that  a  single  rule  may  be  inadequate  to  fully  evaluate  a  complex 
situation,  often  groups  of  rules  called  "rule  sets"  are  written  to  work  together  on 
particularly  difficult  analysis  tasks.  Such  organization  of  rules  allows  a  manageable 
and  modular  approach  to  expert  system  design.  However,  the  random  nature  of  rule 
firing  allowed  by  an  inference  engine  may  permit  partially  processed  facts  to  be 
accessed  and  used  by  other  rules  before  the  original  rule  set  has  completed  the  group 
objective.  For  this  reason  it  is  usually  desirable  to  ensure  that  rule  sets  are  able  to  run 
to  completion  whenever  activated,  before  other  rules  arc  again  allowed  to  fire.  As  an 
example,  implementation  of  the  algorithm  in  Figure  6.6  requires  several  polyhedron 
building  rule  sets  working  together  in  a  coordinated  fashion  with  parametric  regression 
rule  sets. 

Given  the  unpredictable  nature  of  heuristics  when  solving  highly  complex 
problems,  the  expert  system  designer  may  need  to  impose  some  controls  on  execution 
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flow  among  rule  sets  in  order  to  ensure  orderly  execution.  Randomness;  generally 
remains  desirable  and  can  stiti  coexist  within  the  bounds  of  rule  execution  flow  control 
requirements. 

4.  Developing  an  Expert  System 

When  a  new  application  appears  to  be  suitable  for  an  expert  system 
implementation,  the  first  developmental  step  is  to  define  the  application  problem  in 
clearly  understandable  terms.  This  usually  requires  acquisition  of  expert  knowledge  in 
the  problem  area  to  be  solved.  The  facts  that  may  exist  in  the  problem  domain  must 
be  stated  as  unambiguously  as  possible.  The  overall  problem  can  be  logically  grouped 
into  simply  stated  subproblems  consisting  of  condition-action  rules  (Jackson  90). 

Once  the  problem  is  well-defined,  facts  and  rules  are  converted  from  plain 
language  into  the  syntax  of  the  expert  system  being  used.  When  first  building  an 
expert  system,  facts  and  rules  should  be  added  in  small  numbers.  Incrementally  test 
the  expert  system  and  avoid  adding  new  rule  sets  until  examples  show  that  existing 
rules  work  as  intended.  Additional  adjustment  may  be  necessary  to  ensure  mutual  rale 
cooperation  whenever  new  rales  are  added.  Such  am  incremental  prototyping  approach 
can  be  particularly  effective  when  building  large  expert  systems  (Sacerdoti  91). 

G.  IMPLEMENTATION  AND  EVALUATION 

I.  NPS  AUV  Vehicle  Description  and  Sonar  Characteristics 

Naval  officers  and  civilian  scientists  at  NPS  are  conducting  active  research 
using  an  AUV  designed  and  constructed  at  the  school.  The  NPS  AUV  is  used  for 
basic  research  and  thesis  work  in  control  systems  technology,  artificial  intelligence, 
scientific  visualization  and  systems  integration.  Specific  NPS  AUV  project  objectives 
include  the  study  of  mission  planning,  navigation,  collision  avoidance,  real-time 
mission  control,  replanning,  object  recognition,  vehicle  dynamic  motion  control,  and 
post-mission  data  analysis  (Healey  91)  (Bratzman  Compton  91). 

The  NPS  AUV  is  eight  feet  long  and  neutrally  buoyant,  displacing  387 
pounds  with  overall  size  and  shape  comparable  to  a  small  dolphin.  Current  vehicle 
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endurance  is  two  tc*  three  hours.  Maximum  speed  of  die  NPS  AUV  is  about  two 
knots.  The  NPS  AUV  turning  diameter  is  under  three  body  lengths,  designed  to  be 
ideal  for  maneuvering  in  the  large  NPS  swimming  pool.  The  NPS  pool  allows  precise 
testing  in  a  quiet,  controlled  environment.  Opemocean  testing  is:-feasible  but  is  being 
reserved  for  a  more  robust  followron  vehicle.  Video  cUps  showing  normal  NPS  AUV 
operation  are  available  in  (Brutzman,  Floyd,  Whalen  92)  and  (Brutzman  92). 

The  primary  components  of  the  NPS  AUV  are  an  aluminum  hull,  fiberglass 
sonar  dome,  four  high-frequency  directional  sonar  transducers,  twin  counter-rotating 
four-inch  propellers,  lead-acid  batteries,  eight  plane  surfaces,  and  a  Gespac  computer 
running  a  Motorola  68030  processor  with  a  2  MB  RAM  card.  Figure  6.10  shows  a 
general  schematic  of  the  NPS  AUV. 

Four  PSA-900  Programmable  Sonar  Altimeters  made  by  Datasonics  Inc.  are 
orthogonally  fixed  in  the  nose  of  the  NPS  AUV  pointing  directly  ahead, downward 
and  to  port  and  starboard.  These  transducers  are  fixed  frequency  and  ultrasonic,  each 
at  approximately  200  Khz.  Sonar  range  gate  is  selectable  at  30  m  or  300  m,  and  pulse 
length  is  350  ps.  Normal  pulse  repetition  rate  is  10  Hz.  Sonar  beamwidth  is  seven 
degrees  and  range  resolution  is  1  cm  at  30  m. 

2.  CLIPS  Expert  System 

A  number  of  expert  systems  are  commercially  available.  CLIPS  ("C" 
Language  Integrated  Production  System)  was  chosen  for  this  application  due  to  its 
portability,  extendability,  capabilities,  thorough  documentation  and  interactive  tutorials 
(NASA  91).  CLIPS  is  also  reasonably  priced  (approximately  $450  or  free  for 
government  agencies).  CLIPS  was  developed  by  NASA  to  meet  the  varied 
requirements  of  NASA  Mission  Control  Center  delivery  systems.  CLIPS  syntax  is 
similar  to  the  functional  language  Lisp  and  follows  the  if-then  conditional  rule  model. 
The  most  recent  versions  of  CLIPS  add  object-oriented  and  procedural  programming 
capabilities  (Giarratano  91).  Since  it  is  written  in  "C",  CLIPS  can  run  under  most 
computer  architectures.  A  feature  of  CLIPS  that  makes  it  particularly  suitable  for 
AUV  use  is  that  developed  expert  systems  may  be  exported  to  nearly  any 
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Figure  6.10  General  schematic  of  NPS  AUV 


microprocessor  by  autogeneration  of  executable  ”C"  language  code.  CLIPS  has  an 
active  user  base,  annual  applications  conferences,  an  applications  abstract  registry  and 
is  provided  with  complete  source  code  (Brooke  92). 
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3;  NPS  AUV  Sonar  Classification  System 

The  program  used  to  implement  the  concepts  presented  in  this  chapter  was 
written  using  the  CLIPS  expert  system.  Actual  sonar  data  collected  by  the  NPS  AUV 
is  recorded  in  files  for  later  use  as  input  to  the  sonar  classification  expert  system. 

This  sonar  data  is  analyzed  off-line  while  running  on  a  separate  workstation. 

A  variety  of  ouQ)uts  from  the  expert  system  provide  several  ways  to 
visualize  results.  Two-dimensional  graphics  plots  of  raw  sonar  data  and  Corresponding 
parametric  regression  line  segments  are  shown  On  screen  and  as  hard  copy.  An  Output 
file  listing  each  individual  geometric  object  and  classification  provides  botii  hard  copy 
of  results  and  automatic  input  to.the  three-dimensional  NPS  AUV  Integrated  Simulator 
described  below. 

Sonar  geometric  analysis  is  computationally  intensive.  While  running  under 
the  CLIPS,  environment  on  a  Sun  2  workstation,  the  expert  system  is  currently  able  to 
maintain  a  7  Hz  sonar  return  processing  rate.  This  is.  nearly  as  fast,  as  the  10  Hz  data 
rate  recorded  by  the  NPS  AUV  and  adequate  for  most  real-time  requirements. 
Optimization,  elimination  of  network  file  server  bottlenecks  mid  source  code 
compilation  will  further  improveperformance.  Project  goals  include  porting  the 
NPS  AUV  sonar  classification  expert  system  to  a  microprocessor. internal  to  the 
vehicle. 

It  is  clear  that  a  sonar  classification  expert  system  can  operate 
autonomously  in  real  time. 

4.  NPS  AUV  Integrated  Simulator 

Typically  the  development  and  testing  of  AUV  hardware  and  software  is 
greatly  complicated  by  vehicle  inaccessibility  during  operation.  Integrated  simulation 
remotely  links  vehicle  components  and  support  equipment  with  graphics  simulation 
workstations.  Integration  pf  actual  AUV  components  with  three-dimensional 
simulation  allows  complete  real-time,  pre-mission,  pseudo-mission  and  post-mission 
visualization  and  analysis  in  the  lab. 
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Integrated  simulator  testing  of  AUV s  is  a,  broad  and  versatile  method  that 
has  proven  very  effective  in  the  development  of  the  NPS  AUV  sonar  classification 
expert  system  (Brutzman  March  92)  (Brutzman  May  92).  in  jparticular,  post-mission 
simulator  playback  of  recorded  telemetry,,  sonar  sensor  data  and.  system  state 
transitions  supports  in-depth  reenactment,  playback  and  analysis  of  processed  sonar 
data.  This  scientific  visualization  approach  pemiits  rapid  and  precise  development  of 
geometric  analysis  techniques  and  classification  heuristics  for  the  NPS  AUV  sonar 
classification  system. 

High-resolution  three-dimensional  graphics  workstations  can  provide 
real-time  representations  of  vehicle  dynamics,  control  system  behavior,  mission 
execution,  sonar  processing  and  object  Classification.  Use  of  well-defined,  user- 
readable  mission  log  files  as  the  data  transfer  mechanism  allows  consistent  and 
repeatable  simulation  of.  all  AUV  operations. 

Hi  EXPERIMENTAL  RESULTS 

1.  Classification  Test  Scenario 

An  example  best  demonstrates  successful  classification  of  actual  sonar 
returns.  A  single  swimmer  was  chosen  to  represent  a  mine-like  object  and  was 
positioned  as  a  target  near  the  right-hand  wall  of  the  NPS  swimming  pool,  shown  in 
Figure  6.11. 

The  NPS  AUV  was  programmed  to  follow  a  racetrack  traversal  of  the  pool 
and  record  all  pertinent  data.  Figure  6.12  shows  individual  left  transducer  sonar 
returns  plotted  as  circles  and  vehicle  track  as  a  large  oval,  while  the  line  segments 
calculated  by  the  parametric  regression  algorithm  are  shown  superimposed.  Some 
distortion  is  evident  due  to  unmodeled  sideslip  error  in  vehicle  track  data. 

2.  Experimental  Results 

The  sonar  data  recorded  by  the  NPS  AUV  in  the  pool  are  uploaded  after 
mission  completion  via  modem  and  processed  off-line  by  the  authors’  sonar 
classification  expert  system.  Classification  results  are  then  graphically  rendered  by  the 
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NPS  AUV  fiitegrated  Simulator  running,  on  a  Silicon  Graphics  Iris  workstation.  This 
three-dimensional  display  shows  all  generated  parametric  regression  line  segments, 
inferred  edges,  hidden  edges,  and  detected  walls,  the  overall  pool  graphics, display  as 
seen  from  a  viewpoint  high  above  the  pool  is  shown  in  Figure  6113.  The  target  of 
interest  met  classification  criteria  for  a  minerlike  object  and  a  simulation  closeup  is 
shown  in  Figure  6.14. 

The  integrated  simulator  has  the  additional  feature  of  being  able  to  play 
back  sonar  detections  and  classifications  simultaneously  with  vehicle  motion  in  real 
time  or  slow  motion.  Evaluation  of  sonar,  classification  results  using  the  scientific 
visualization  techniques  provided  by  the  integrated  simulator  was  extremely  helpful 
during  development  and  testing  of  sonar  expert  system  classification  heuristics. 

The  experimental  results  show  that  the  NPS  AUV  Autonomous  Sonar 
Classification  System  is  highly  effective  at  classifying  objects  despite  the  low 
resolution  of  the  active  sonar  employed. 
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Figure  6.12  NPS  AUV  sonar  classification  expert  system  plot  of  pool  data 
and,  parametric  regression  line  segments 

I.  DISCUSSIONS  AND  APPLICATIONS 

1.  Extendability  to  Video,  Lasers,  Complex  Sonars  and  Sensor  Fusion 
Active  sonar  is  not  the  exclusive  sensor  used  for  underwater  object 
detection  and  classification.  A  variety  of  other  sensors  are  coming  into  use  including 
underwater  videocameras  and  lasers.  In  addition  to  range  and  bearing  data,  advanced 
sonars  may  provide  completely  different  types  of  data  such  as  frequency  spectra, 
doppler  or  long-range  conical  beam  data.  Ultrasonic  sonars  have  also  been  employed 
by  land  vehicles. 

All  of  these  sensors  share  common  characteristics  that  allow  autonomous 
analysis  by  expert  systems.  Each  sensor  type  provides  data  sets  that  can  be  analyzed 
using  geometric  reasoning  techniques.  In  every  case  expert  knowledge  can  define  both 
quantitative  and  heuristic  rules  for  processing  sensor  outputs  to  create  primitive 
geometric  objects,  thus  allowing  object  classification. 


86 


Figurc6,13  Iritcgratcd'Simulator  scrccn  display  of  the  fuU  NPS  pool  and  all 
sonar  classifications 

Sensor  fusion  is  the  correlation  of  multisource  information  to  resolve 
ambiguity,  and  increase  confidence  in  individual  classifications.  Sensor  fusion  is 
particularly  valuable  in  offsetting  the  weaknesses  of  one  sensor  type  with  the  strengths 
of  another.  An  example  of  sensor  fusion  might  be  to  correlate  accurate  laser  bearing 
data  with  accurate  sonar  range  data.  A  thorough  survey  on  multisenspr  fusion  roles, 
approaches  and  applications  is  provided  by  (Luo  89).  Sensor  fusion  can  be  directly 
implemented  using  the  pattern-matching  capabilities  of  a  multisensor  classification 
expert  system. 

2.  Intelligent  Remote  Sensors 

The  use  of  remote  sensors  is  becoming  commonplace.  The  primary 
limitation  of  most  remote  sensors  is  that  they  have  little  ability  to  independently  react 
to  sensor  inputs.  Most  sensing  devices  require  direct  control  or  have  an  arbitrary 
sampling  period,  while  continuously-sensing  devices  require  dedicated  data 
communication  lines.  Remote  underwater  sensors  need  to  operate  autonomously  or 


Figure  6.14  Integrated  simulator  display  close-up.  of  a  mine-like  object 
classified  by  the  soiiar  expert  system  using  detected  edges, 
inferred  edges,  hidden  edge  and  cross-sectional  area 


with  minimal  external  control  in  order  to  improve  their  efficiency,  capabilities  and 
cost-effectiveness.  Embedding  an  expert  system  application  using  , 
microprocessor-based  control  is  a  feasible  method  to  create  intelligent  and  autonomous 
remote  sensors. 

3.  Data  Reduction 

Most  sensor  data  is  high  bandwidth.  Autonomous  vehicles,  remotely 
operated,  vehicles  and  remote  sensors  typically  receive  extremely  large  amounts  of 
data.  Storage  or  transmission  of  raw  data  for  off-line  processing  is  undesirable  and 
imposes  unreasonable  memory  capacity  and  communications  requirements;  A 
significant  benefit  of  autonomous  classification  is  that  it  reduces  massive  amounts  of 
raw  data  into  concise  information  that  can  be  efficiently  recorded  or  communicated. 
Data  without  value  is  easily  filtered.  The  overall  data  compression  ratio  can  equal 
several  orders  of  magnitude. 

4.  Future  Use  of  Expert  Systems  by  Autonomous  Vehicles 

If  autonomous  vehicle  sensors  and  missions  are  to  become  increasingly 
capable  and  sophisticated,  it  is  likely  that  parallel  processing  of  distributed  artificial 
intelligence  modules  will  be  necessary  in  order  to  provide  adequate  computing  power 
with  real-time  response.  A  typical  set  of  high-level  processes  might  include  detection 
and  classification  for  multiple  sensors,  path  planning,  search,  systems  control  and 
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others.  None  of;  these  processes  is  completely  indejtendent,  but  typically  each  process 
can  run  in  paraUel  with;  the  others  most  of  the  time.  One  abstract,  software 
architecture  that  supports  such  a  distributed  approach  is  the;  blackboard  paradigm. 

A  blackboard  system  directly  extends  thefunctionality  seen  in  an  expert 
system  for  a  collection  of  distributed  processes  (Jackson  90):  A  good  metaphor  for  die 
blackboard  approach  is.  a  group  of  human  experts  working  together  on  a  large  problem 
using- a  blackboard  as  their  means  of  communication.  Problem  definition,  data, 
questions  and  answers  Can  all  be  written  arid  read  pn  various  sections  of  the 
blackboard.  Each  independent  expert  has  full  access  to  the  blackboardand  looks  for 
information  pertinent  to  his  area  of  expertise.  When  an  expert  develops  some  result  or 
new  question  worth  communicating  to  the  group,  that  information  is  recorded  on  the 
blackboard. 

Similarly,  a  blackboard  system  has  distributed  independent  knowledge 
sources,  each  of  which  can  use  any  method  desired  to  solve  portions  of  a  large 
problem.  Communications  are  recorded  on  the  blackboard  and  are  available  to  all 
knowledge  sources.  Complex  problems  are  solved  through  cooperative  reasoning 
(Coridll  91).  As  another  example,  each  of  the  processes  shown  in  Figure;6.1  might  be 
implemented  as  separate  knowledge  sources  for  a  blackboard.  Expert  systems  are  well 
suited  as  knowledge  sources  for  the  blackboard  paradigm. 

Development  of  autonomous  expert  systems  is  likely  to  provide  intelligent 
components  that  will  remain  useful  in  the  advanced  architectures  of  future  autonomous 
vehicles. 

J.  CONCLUSIONS 

Autonomous  sonar  classification  systems  can  accurately  detect  and  classify 
objects  in  the  underwater  environment.  Precise  geometric  analysis  is  combined  with 
qualitative  expert  system  heuristics  to  provide  a  flexible  and  robust  approach  with 
wide  applicability.  Autonomous  classification  systems  are  capable  of  supporting 
sophisticated  real-time  applications  in  working  autonomous  vehicles. 
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VIL  SHORTEST  PATH  PLANNING  USING  A  CIRCLE  WORLD 
A.  ABSTRACT 

Path  planning  is  a  critical  capability  for  mobile  robots  operating  iri  environments 
containing  obstacles.  Building  a  path  planning  module  as  part  of  this  thesis  has  been 
helpful  in  understanding  design  requirements,  world  models  and  software  architecture 
specifications  both  for  the  NPS  AUV  and  the  NPS  AUV  Integrated  Simulator. 

Mobile  robofcpath  planning  around  obstacles  can  be  accomplished  by  modeling 
obstacles  as  pairs  of  identical  circles  with  opposite  rotations.  Addition  of  robot  radius 
and  safety  standoff  distances  to  circle  radii  allows  modeling  the  robot  as  a  point.  This 
circle  world  model  can  be  used  to  calculate  shortest  paths  between  points. 

Tangents  between  individual  circles  in  a  circle  world  have  no-inherent 
redundancy  due  to  the  geometric  uniqueness  of  each  landing  and  leaving  point  on 
every  circle.  Alternate  partial  paths  landing  at  an  intermediate  circle  obstacle  must  be 
properly  compared  in  order  to  determine  which  is  shortest.  Dijkstra’s  algorithm  or 
(preferably)  A*  search  can  selectively  use  visibility  and  .partial  path  comparison 
calculations  to  find  the  shortest,  safest  or  optimal  path  between  start  and  goal  points. 

Tangent  visibility  from  a  single  ppint  to  all  circles  can  be  calculated  in 
order  Q(nlogn)  time.  Similarly,  tangent  visibility  from  a  single  circle  to  all  other 
circles  can  be  calculated  in  order  O(nlogn)  time.  The  shortest  path  between  start  and 
goal  points  can  be  calculated  in  order  0(n2log«)  time. 

Since  obstacle  avoidance  is  a  typical  robot  behavior  regardless  of  obstacle 
height,  the  circle  world  search  model  is  directly  extendable  to  the  general  case  of 
three-dimensional  path  planning.  This  approach  is  shown  to  be  particularly  suitable 
for  underwater  vehicle  path  planning. 
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B.  INTRODUCTION  AND  PROBLEM  DESCRIPTION  OF CIRCLE  WORLD 

Robot  path  planning  is  the  search  for  an  allowable,  safe  or  optimal  path  for  a 
robot  to  follow  from  one  location  to  another.  A  critical  consideration  in  robot  path 
planning  is  the  choice  of  model  to  represent  the  obstacles  that  a  robot  must  avoid. 

.  A  well-known  and  fundamental  metiiod  used  for  path  planning  is  exemplified  by 
configuration  space  approach  (Lozano-P&ez  79)  (Yap  87)  (Akman  87)  (Laumond  87) 
(Schwartz  88)  (Canny  88).  In  the  configuration  space  approach  a  world  is  modeled  as 
a  set  of  geometric  obstacles.  Obstacle  boundaries  are  grown  to  include  the  effective 
radius  ofamobile  robot  The  mobile  robot  center  can  then  be  treated  as  a  reference 
point  Any  location  in  the  remaining  non-obstacle  free  space  is  considered  a  legal 
position  for  the  mobile  robot  reference  point  Visible  tangents  can  then  be  calculated 
between  all  obstacles.  A  fully  connected  graph  is  defined  by  obstacle  boundaries  and 
the  tangents  between  them.  Determination  of  a  shortest  path  is  accomplished  by 
searching  the  visibility  graph  for  the  lowest  cost  path  between  start  and  goal  points. 
Polygons  are  typically  the  geometric  form  chosen  to  represent  obstacles. 

Another  simple  and  effective  way  to  represent  obstacles  in  a  configuration  space 
world  model  is  io  draw  circles  around  them.  The  center  coordinates  of  each  circle  in 
the  circle  world  model  are  located  at  the  centroid  of  each  corresponding  obstacle.  The 
initial  radius  of  each  circle  equals  the  minimum  radius;  which  completely  surrounds  the 
given  obstacle  (Figure  7.1).  The  maximum  radius  of  the  moving  robot  is  combined 
with  desired  safety  standoff  distance  and  added  to  each  obstacle  circle  radius 
(Figure  7.2). 

Such  a  circle  world  model  allows  a  mobile  robot  to  be  represented  as  a  moving 
point.  Any  path  in  this  circle  world  which  can  be  drawn  without  crossing  the  interior 
of  a  circle  boundary  is  a  valid  robot  path. 

As  the  number  of  obstacles  in  a  circle  world  increases,  the  possible  number  of 
tangential  paths  from  a  start  point  to  a  goal  point  rises  exponentially  due  to 
combinatorial  explosion.  A  simple  circle  world  that  includes  all  visible  circle  tangents 
quickly  becomes  crowded  (Figure  7.3).  A  typical  circle  world  of  moderate  density  has 
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Figure  7.1 


Simple  obstacle  representation  using  circles 


about  half  of  the  circle  tangents  traversable  while  the  remaining  tangents  are  blocked. 

The  normal  objective  of  path  planning  through  an  obstacle  field  is  robot  travel 
from  a  known  start  point  to  a  known  goal  point  Typically  a  determination  of  shortest 
path  is  desired,  and  therefore  Euclidean  distance  traveled  is  the  metric  used  to 
determine  the  best  route  from  start  to  goal  (Figure  7.4).  Other  path  selection  criteria 
such  as  safety  or  optimality  may  also  be  considered  (Kanayama  88).  The  circle  world 


Figure  7.3 


Simple  circle  world  with  all  visible  tangents 


Figure  7.4 


Simple  circle  world  shortest  path 


model  and  path  planning  algorithm  presented  here  allow  rapid  and  efficient 
determination  of  shortest-distance  paths. 

At  least  one  previous  application,  the  Stanford  cart  mobile  robot,  used  a  similar 
circle  world  model  for  obstacle  representation  and  avoidance  (Moravec  80) 

(Moravec  83).  However,  the  combination  of  vision  processing  and  path  planning 
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aboard  that*  small  robot  proved  prohibitively  slow  for  real-time  use  due  to  hardware 


limitations  and  greater  algorithmic  complexity. 

Although  most  configuration  space  treatments  are  based  solely  on  linear  or 
polygonal  obstacles,  obstacles  composed  of  any  closed  combination  of  line  segments 
and  circular  arcs  are  formally  addressed  in  (Laumond  87).  However  that  work  does 
not  explore  the  complexity  differences  between  circle  worlds  and  polygonal  worlds  for 
tangent  computation  and  shortest-path  search  algorithms. 

It  is  expected  that  the  shortest-path  planning  algorithm  provided  in  this  paper 
will  support  real-time  path  planning  by  autonomous  robots.  These  results  are 
presented  with  additional  mathematical  theory  and  greater  detail  in  (Kanayama 
Brutzman  91),  which  is  included  as  Appendix  D. 


C.  GEOMETRIC  CHARACTERIZATIONS  OF  CIRCLE  WORLD  AND 

SHORTEST  PATH 

In  a  circle  world,  obstacles  are  modeled  by  surrounding  them  with  circles. 
However,  each  circle  surrounding  an  obstacle  can  have  two  possible  traversal  rotation 
modes:  clockwise  or  counter-clockwise.  In  keeping  with  convention  (Kanayama  91), 
clockwise  traversals  of  circle  perimeters  can  also  be  referred  to  as  minus  or 
left-handed,  while  counter-clockwise  traversals  can  be  referred  to  as  plus  or 
right-handed.  A  two-dimensional  space  filled  with  n  noncontiguous  obstacles  can  be 
fully  represented  by  n  clockwise  circles  and  n  corresponding  counter-clockwise  circles. 
Thus  each  obstacle  is  represented  by  two  circles  with  opposite  rotation  modes,  both 
centered  at  the  coordinates  of  the  obstacle  centroid. 

The  geometric  data  primitives  needed  to  fully  represent  a  circle  world  are  Point, 
Segment,  Circle,  Tangent,  Arc,  Configuration  and  Path.  These  geometric  primitive 
data  structures  are  defined  here  using  a  hierarchical  approach  for  simplicity  and 
clarity.  When  capitalized,  these  terms  refer  to  the  explicitly  defined  data  structures 
summarized  in  Table  VII.l.  These  data  structure  definitions  are  included  in  order  to 
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best  explain  the  circle  world  problem  as  well  as  the  circle  world  source  code  in 
Appendix  E.  L 

Table  mi  CIRCLE  WORLD  GEOMETRIC  DATA  STRUCTURES 


Circle  World  Geometric 
Primitives  Data  Structures 

Structure  Elements 

Element  Data  > 
Types 

Point 

x,y 

float 

Segment 

pointl,  point2 

Point 

Circle 

center 

radius 

direction 

Point 

float 

CW  ( -1), 
CCW  (+1)  or 
POINT  (  0) 

Tangent 

circle 

Circle 

angle 

float 

Arc. 

circle 

angle  1,  angle2 

Circle 

float 

Configuration 

tangent 

orientation 

Tangent 

float 

Path 

initial_segment 

•  •• 

Segment 

Mt 

(note:  arc-segment  pairs  are 

arc 

Arc 

repeated  as  necessary) 

segment 

Segment 

A  Point  in  a  circle  world  is  defined  by  a  set  of  two-dimensional  Cartesian 
coordinates.  A  Segment  is  the  line  segment  defined  by  two  points  that  are  connected 
by  a  straight  line.  A  Circle  data  structure  is  defined  by  a  Point,  a  radius  and  a 
direction  of  rotation.  A  Tangent  data  structure  is  comprised  of  a  Circle  and  an  angle 
which  is  the  orientation  between  the  circle  center  and  the  tangent  point  on  the  circle 
circumference.  An  Arc  is  defined  by  a  Circle  and  two  angles,  corresponding  to  the 
starting  and  finishing  angles  oriented  from  the  circle  center  to  the  arc  starting  and 
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finishing  endpoints;  Note  that  Arc  direction  of  rotation  is  not  dependent  on  the 
precedence  of  the  starting  and  finishing  angles,  but  rather  is  implicitly  included  in  the 
Arc  by  the  rotation  direction  of  the  member  Circle.  A  Configuration- is  a  combination 
of  a  Tangent  and  an  orientation  angle.  The  orientation  angle  of  a  Configurationis 
always  perpendicular  to  the  component  Tangent  angle  but  can  be  in  either  of  two 
directions.  Thus  a  Configuration  point  defines  whether  a  Tangent  is  a  landing  point  or 
a  leaving  point.  Finally; -a  Path  between  a  pair  of  start  and  goal  points  in  a  circle 
world  includes  a  Segment  followed  by  zero  or  more  Arc-Segment  pairs.  A  typical 
Path  includes  a  straight  line  segment  from  the  start  point  to  the  first  circle,  obstacle,  an 
Arc  around  a  portion  of  that  circle,  a  Segment.frdm  the  first  circle  to  the  next  circle 
and  so  on  until  the  desired  goal  point  is  reached.  As  an  example,  a  single  segment 
Path  could  directly  connect  the  start  and  goal  points  if  no  circle  obstacles  were 
between  them. 

A  special  case  arises  that  is  of  use  when  designing  visibility  algorithms:  a  Point 
can  also  be  treated  as  special  case  of  a  Circle.  Setting  a  Circle  radius  value  to  zero  or 
rotation  mode  to  zero  (or  both)  effectively  makes  that  Circle  behave  as  a  Point.  In 
particular  this  approach  allows  treating  the  start  and  goal  points  as  circles,  simplifying 
evaluation  of  tangents  and  paths  in  a  circle  world. 

Every  pair  of  circles  defines  four  mutual  tangent  line  segments:  two  external 
tangents  and  two  cross-tangents  (Figure  7,5).  Note  that  the  eight  tangent  points  on  the 
circumferences  of  these  two  circles  are  unique.  Also  note  that  the  respective  lengths 
of  the  external  tangent  segments  and  cross-tangent  segments  are  equal. 

The  coordinates  of  the  four  possible  tangents  between  the  pairs  of  circles  shown 
in  Figure  7.6  can  be  calculated  using  Equations  (7.1),  (7.2)  and  (7.3).  Rotation  mode 
values  follow  the  convention  counter-clockwise  (CCW,  right-handed,  plus  or  +1)  and 
clockwise  (CW,  left-handed,  minus  or  -1).  Points  which  are  being  modeled  as  special 
cases  of  circles  have  rotation  mode  and  radius  equal  to  zero. 

Angle  8  in  Equation  (7.1)  and  Figure  7.6  is  the  offset  angle  between  the 
circle-center-to-circle-center  orientation  0  and  tangent  orientation  a. 
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Tangential  line  Segments 

ll  -  => 


lr,  rl  tangents  are  "cross-tangents" 

11,  rr  tangents  are  "external  tangents" 


Figure  7.5  Tangential  line  segments  between  circles 

mode ,  •  r,  -  mode.  •  r, 

6  =  arcsin - - - - - -  --- — r 

distance(circlevcenter,  circle2.ceiiter) 
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Circle  Tangent  Determination 


0  -  orientation  (drdel  .centered rdo2. center) 
a  -  normalize  (0  -  $) 


Figure  7.6  Determination  of  circle  cross-tangents  and  external  tangents 
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Angle  0;in  Equation  (7.2)  and^igure  1.6  is  the  orientation  betwwnarcle 
centers. 


0  =  orientation  (circlevcenter,  drcle2.center)  (74) 


Angle  a  in  Equation  (7.3)  and  Figure  7.6  is  the  orientation,  of  the  desired. tangent 


between  the  circles: 


a  =  normcMze  (0  -  6) 


(73) 


Visibility  in  circle  world  is  defined  as  the  ability  to  connect  two  points  using  a 
single  line  segment  without  crossing  any  circle  boundary.  The  first  step  in 
determining  visibility  is  to,  evaluate  point-to-point  segment  orientation  relative  to  every 
circle  center  using;Equations  (7.4),  (7:5)  and  (7.6).  The  three  regions  of  possible 
circle,  locations  relative  to  the  point-to-point  segment  are  shown  in  Figure  7.7. 

Angle  0  in  Equation  (7.4)  arid  Figure  7.7  is  the  orientation  between  the  two 
points  being  checked  for  visibility. 

0  -  ojientation(pointl,  point2)  (7.4) 

Angle  81  in  Equation  (7.5)  and  Figure  -1.1  is 'the -angle  between  the  currentcircle 
arid  line  segment  left  side. 

8j  =  orientationfpoinil,  drclevcenter )  -  0  (7.5) 

Angle  82  in  Equation  (7.6)  and  Figure  7.7  is  the  angle  between  the  current  circle 
and  line  segment  right  side. 

82  =  orientation(point2,  circle vcentef)  -  0  (7.6) 

To  complete  the  visibility  check  for  two  points  and  a  given  circle,  the  circle 
radius  is  now  compared  to  the  distance  between  the  closest  endpoint  and  the  circle 
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center.  Three  comparison  results  are  possible.  In  the  left-most  repon  of  Figure  7.7, 
successful  test  of  Inequality  (7.7)  indicates  that  the  current  circle  radius  should  be 
compared  to  the  distance  to  poind  for  visibility  determination. 

IM  >  \  <7-7> 
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In  the  center  region  of  Figure  7.7,  successful  test  of  both  Inequalities  (7.8)  and. 
(7.9)  indicate  that  the  circle  radius  in  question  should  be  compared  to  the  distance  to 
both  pointl  and  point?  for  visibility  determination. 

18,1*1  O*) 

l»2l>f  09) 

In  the  right-most  region  of  Figure.  7.7,  successful  test  of  Inequality  (7.10) 
indicates  that  the  current  circle  radius  should  be  compared  to  the  distance  to  point2  for 
visibility  determination. 

1**1  i.|  O10) 

This  segment  visibility  determination  process  must  be  repeated  until  any  circle 
intersection  disproves  visibility,  or  until  all  circles  in  the  circle  world  have  been 
checked  without  intersection.  A  segment  that  touches  only  the  periineter  of  a  circle 
without  crossing  the  circle  boundary  can  be  considered  visible.  Visibility 
determination  for  a  single  pair  of  points  in  a  world  containing  order  0 (»)  circles  has 
algorithmic  complexity  of  order  O («). 

The  primary  objective  of  path  planning  is  to  find  a  safest  or  shortest  path.  A 
simple  measure  of  path  costs  in  a  circle  world  is  the  straightforward  summation  of 
Euclidean  distances  along  segments  and  arcs.  However,  comparing  relative  costs 
when  two  partial  paths  have  different  landing  points  on  the  same  circle  is  more 
complex.  Although  the  first  partial  path  can  reach  the  intermediate  circle  obstacle 
using  a  shorter  route  than  a  second  partial  path,  the  second  partial  path  can  be  part  of 
u  shorter  overall  route  to  the  goal  when  travel  around  the  current  intermediate  circle 
circ’iro.ference  is  included.  Inorderto  determine  which  partial  path  which  will  be  in 
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the  shortest  overall  path,  the  two  partial  paths  must  be  properly  compared  (Figure  7.8). 
An  accurate  comparison  can  be  obtained  by  including  the  cost  of  the  arc  between  the 
landing  points  of  the  two  arriving  partial  paths.  Application  of  Inequality  (7.11)  in 
Figure  7.8  indicates  which  partial  path  is  shorter. 

? 

costfpathl )  +  axc_cost(a,  b,  mode )  <  cost(path2)  (7.11) 

Note  that  such  a  comparison  is  only  meaningful  when  each  partial  path  follows 
the  same  direction  of  rotation  around  the  intermediate  circle  being  evaluated.  The 
definitions  of  Circle  and  Arc  explicitly  include  direction  of  rotation.  Opposite  rotation 
modes  around  the  same  obstacle  are  part  of  opposite  circles  and  thus  mutually 
exclusive  paths.  Should  the  leaving  point  for  the  actual  shortest  path  lie  between  die 
two  partial  path  landing  points  (e.g.  points  a  and  b  of  Figure  7.8),  the  longer  of  the 
arriving  partial  paths  will  correctly  be  excluded. 

Successful  determination  of  visibility  and  correct  comparison  of  intermediate 
path  costs  for  multiple  points  and  circles  allows  the  employment  of  search  techniques 
to  find  the  best  path  overall  from  start  to  goal.  Formal  statement  and  proof  of  the 
path  comparison  proposition  can  be  found  imAppendix  F  section  4.3  (Kanayama 
Brutzman  91). 

D.  ALGORITHM  FOR  DETERMINING  VISIBLE  TANGENTS 

Assume  that  a  general  circle  world  is  . modeling  n  independent  obstacles  with  2 n 
circles.  Visibility  determination  for  a  single  pair  of  points  in  this  world  has  already 
been  shown  to  have  algorithmic  complexity  of  order  0 (n).  The  set  of  all  pairs  of 
points  that  may  need  to  be  checked  includes  line  segments  from  the  start  point  to  the 
finish  point,  start  point  to  a  tangent  on  every  circle,  finish  point  to  a  tangent  on  every 
circle,  and  four  mutual  tangents  between  each  possible  pair  of  circles.  Tangents  are 
not  possible  between  a  circle  and  itself  or  its  opposite.  As  a  result,  a  total  of 
[4(2n)(2n  -  2)  +  4it  +  1]  =  [16/r2  -  \2n  +  1]  possible  line  segments  may  need  to  be 
evaluated. 


Comparison  of  Partial  Path  Costs 


pathl 


? 

cost  (pathl)  +  arc_cost  (a,  b,  CCWJ  <  cost  (path2) 

Figure  7.8  Comparison  of  partial  path  costs 

Since  there  is  no  inherent  geometric  redundancy  in  any  of  these  tangent  points, 
each  tangent  segment  must  be  considered  independently  of  all  others.  However  there 
is  no  requirement  that  every  possible  tangent  segment  be  evaluated  when  determining 
the  shortest  path.  The  proper  choice  of  which  points  and  segments  need  to  be 
evaluated  is  essential  in  order  to  conduct  an  efficient  search. 

Visible  tangents  from  a  single  point  to  all  other  circles  can  be  determined  using 
a  sweep  method  (Preparata  85)  (Figure  7.9)  in  order  0(nlogn)  complexity.  The  point 
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to  all  circles  visibility  determination  algorithm  is  elaborated  in  Figures  7.10  and  7.11. 
As  tangents  to  each  circle  are  evaluated,  they  are  inserted  into  and  removed  from  a 
heap  in  order  to  be  sorted  by  orientation.  A  second  pass  of  the  sorted  circle  tangent 
data  employs  heap  insertion  and  deletion  for  direct  determination  of  visibility  of  each 
^tangent.  .  . 

Visible  tangents  from  a  single  circle  to  all  other  circles  (Figure  7.12  and 
Figure  7.13)  can  also  be  determined  using  this  sweep  method  in  order  Ofnlogn) 
complexity.  The  single  circle  to  all  circles  visibility  algorithm  is  nearly  identical  to 
the  point  sweep  algorithm  elaborated  in  Figures  7.10  and  7.11.  Care  must  be  taken  to 
require  proper  matching  of  rotation  mode  values  between  sweep  direction  and  the 
sweep  circle  when  implementing  the  algorithm. 

Similar  sweep  method  algorithms  have  been  used  for  determining  line  segment 
visibility  (Welzl  85)  and  polygon  visibility  (Asano  85)  in  order  0(n2)  time.  It  is 
interesting  to  note  that  substitution  for  each  circle  by  a  line  segment  connecting  the 
left  and  right  tangents  to  that  circle  appears  to  make  this  type  of  sweep  nearly 
identical  to  the  individual  line  segment  visibility  sweep  in  (Welzl  85).  However,  each 
individual  circle  has  order  0(h)  such  tangent  landing  and  leaving  points  associated 
with  it.  Thus,  a  circle  world  has  at  least  order  O(n)  more  segments  in  the  complete 
tangent  visibility  graph  than  a. polygonal  world  or  line  segment  world.  This  increased 
complexity  is  a  significant  difference  between  the  geometry  of  the  circle  world 
problem  and  the  standard  polygon-based  configuration  space  treatments  referenced 
previously. 

E.  SHORTEST-PATH  DIJKSTRA  AND  A*  SEARCH  ALGORITHMS 

Dijkstra’s  algorithm  is  a  standard  approach  to  solving  the  single-source 
shortest-path  planning  problem  (Manber  89).  Given  a  start  point  in  a  circle  world, 
proper  application  of  Dijkstra’s  algorithm  will  calculate  the  shortest  path  to  each 
clockwise  and  counter-clockwise  circle  until  the  designated  goal  point  is  reached.  This 
is  a  greedy  algorithm  in  that  the  shortest  available  path  from  the  start  point  is  always 
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Figure  7.9  Sweep  visibility' determination  from  point  to  all  circles 

selected  upon  each  iteration.  Essentially  the  algorithm  branches  outward  equally  in  all 
directions. 

While  this  algorithm  is  useful  for  building  a  complete  collection  of  shortest  paths 
to  all  circle  obstacles,  it  is  extremely  inefficient  if  only  the  shortest  path  to  a  single 
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O(nlogrt)  Calculate  and  sort  circle  tangents: 

Calculate  left  and  right  tangents  to  each  circle.  Note  that  CCW  and  CW 
rotations  denote  separate  independent  circles. 

After  calculating  a  circle’s  tangents,  use  heap  insertion  to  sort  the  circle 
relative  to  others  by  orientation.  If  two  orientations  are  equal,  secondary 
sort  key  is  shortest  tangent  distance. 

Keep  track  which  is  shortest  right  tangent. 

0(nlogn)  Perform  circular  sweep  to  determine  tangent  visibilities: 

Initialize  sweep  termination  angle  equal  to  sweep  starting  angle. 

Starting  at  circle  with  shortest  right  tangent,  conduct  a  complete  sweep  in 
counter-clockwise  direction. 

Meet  right  tangent:  insert  circle  into  heap. 

If  circle  inserted  is  on  top  of  heap  (or  tangent  distance  equals  top  of 
heap),  mark  that  right  tangent  as  visible.  Otherwise  if  circle  was  below 
top  of  heap,  mark  right  tangent  as  nonvisible. 

Meet  left  tangent:  remove  circle  from  heap. 

If  removed  circle  was  on  top  of  heap  (or  tangent  distance  equaled  top  of 
heap),  mark  that  left  tangent  as  visible.  Otherwise  if  circle  was  below  top 
of  heap,  mark  left  tangent  as  nonvisible. 

If  circle  with  current  .left  tangent  was  not  found  in  heap,  it  overlaps  the 
sweep  starting  angle.  Change  sweep  termination  angle  to  current  sweep 
angle. 

Update  circle  heap  pointer  cross-reference  table. 

Similarly  process  any  other  circle  tangent(s)  at  current  sweep  angle. 
Increment  sweep  angle  to  next  circle  in  sorted  circle  table  and  repeat  for 
all  circles. 

If  necessary,  resume  at  start  of  circle  table  and  continue  sweep  until  all 
tangents  through  the  sweep  termination  angle  are  reprocessed.  This 
allows  any  circles  which  overlap  the  start  angle  to  be  correctly  evaluated. 

Figure  7.10  Explanation  of  sweep  visibility  algorithm  from  point  to  all  circles 

goal  point  is  desired.  If  the  robot  is  repeatedly  operating  in  a  fixed  environment  and 
must  frequently  return  to  a  specific  location,  however,  a  single  application  of 
Dijkstra’s  algorithm  using  that  specific  location  as  a  start  point  will  provide  all 
shortest  paths  past  all  fixed  circle  obstacles.  Precalculation  of  all  shortest  paths  of 
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Algorithm  Sweep_Yisibility JromJPoint  (point,  circlejvprld); 

Input;  Point  from  which  visibility  is  to  be  checked,  and  circle_world  which 
includes  start  point,  goal  point,  and  both  CW  and  CGW  rotations  of  all 
circles. 

Output;  All  cross-tangents  and  external  tangents  originating  at  point,  including 
lengths,  endpoint  coordinates,  orientations  and  visibilities. 

begin 

n  :=  2  (#eircles)  +  2;  {include  CW/CCW  circles,  start  &  goal  points} 
for  i  :=  1  to  n  do 

tangent  :=  calculate  tangent  (point,  circle  [i]); 
push  (tangent,  tangent_heap); 

{primary  sort  key  is  orientation,  secondary  sort  key  is  tangent  length) 
if  tangent, length  <  shortestjangent.length  then 
shortestjangent  :=  tangent; 

for  i  ;=  1  to  n  do  sorted_tangents  [i]  :=  pop  (tangentjieap); 

end_angle:=  shortest_tangent.orientation; 
i  :=  shortestjangent  index  in  sorted jangents  list; 

while  sortedjangents  [ij.orientation  precedes  or  equals  end_angle  do 
if  sortedjangents  [ij.mode  =  RIGHT  then 
push  (sortedjangents  [i],  circle_heap); 

{sort  key  is  length} 

if  (top  (circle_heap).distance  =  sortedjangents  [ij.distance)  then 
sortedjangents  [invisible  ;=  VISIBLE; 
else  sortedjangents  [i]  .visible  :=  NONVISIBLE; 

if  sortedjangents  [i].mode  =  LEFT  then 

if  not  found  (sortedjangents  [i],  circlejieap)  then 
end_angle  :=  sorted_tangents  [i]. orientation; 

{continue  sweep  until  all  circles  fully  evaluated} 
else  if  (top  (circlejieap).distance  =  sortedjangents  [i].distance)  then 
sortedjangents  [invisible  :=  VISIBLE; 
else  sortedjangents  [invisible  :=  NONVISIBLE; 
pop  (sortedjangents  [i],  circlejieap); 
i  :=  (i  +  1)  mod  (n  +  1);  {look  at  next  circle  in  ordered  list} 
end 

Figure  7.11  Pseudocode  for  sweep  visibility  algorithm  from  point  to  all  circles 


107 


interest  may  be  more  cost-effective  than  recalculating  the  latest  shortest  path  objective 
immediately  prior  to  travel. 

In  the  case  of  circle  world  search,  the  A*  search  algorithm  overcomes  the 
inefficiencies  of  Dijkstra’s  algorithm  by  applying  an  evaluation  function  to  each 
iterative  selection  in  order  to  proceed  as  directly  as  possible  towards  the  goal. 
Distance  remaining  to  the  goal  and  arc  cost  around  the  current  circle  obstacle  are 
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Figure  7.13  Sweep  visibility  determination  from  counter-clockwise  circle  to 
all  circles 


included  with  path  cost  when  determining  which  path  of  the  many  available  should  be 
extended  next  (Figure  7.14).  This  search  approach  has  the  effect  of  driving  the  search 
in  the  direction  of  the  goal  and  ignoring  paths  that  are  expensive,  i.e.  paths  that  are 
heading  in  a  wrong  direction. 

The  evaluation  function  comparison  test  applied  in  a  circle  world  search  is  given 
in  Inequality  (7.12)  and  Figure  7.14.  Note  that  the  evaluation  function  term  for 
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A  Evaluation  Function  Comparison 


pathl  path2 


cost  (pathl)  +  arc_cost  (a,  b,  CW)  +  distance  (b,  goal)  > 
cost  (path2)  +  arc_cost  (c,  d,  CCW)  +  distance  (d,  goal) 

Figure  7.14  A*  search  evaluation  function  comparison 


projected  tangent  segment  distance  from  the  current  circle  to  the  goal  point  does  not 
require  the  projected  tangent  segment  to  be  visible. 

? 

costfpothl )  +  arc _cost(at  b,  mode )  +  distance  (b,  goal)  i 


cost(path2 )  +  axc_cost(c,  d,  mode)  +  distanceid,  goal)  (7.12) 
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As  expected,  the  A*  search  algorithm  is  much  more  efficient  thah  Dijkstra  search 
in  a  circle  world.  Given  a  reasonably  uniform  distribution  Of  n  circle  obstacles  about 
a  start  point,  Dijkstra’s  algorithm  tends  to  reachthe  goal  in  order  0(n)  search  steps 
(Figure  7.15).  ' 


It  is  conjectured  that  an  A*  search  requires  only  order  O(logn)  search  steps  for 
the  same  uniformly  distributed  circle  world  (Figure  7.16).  Algorithmic  efficiency  is  a 
critical  consideration  in  the  path  planning  problem,  since  the  total  number  of  tangent 
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segments  in  the  visibility  graph  is  quite  large.  For  the  twenty  circles  of  the 
challenging  example  circle  world,  684  of  1682  total  tangents  (40.7%)  are  visible 
(Figure  7.17). 


Worst  case  A*  search  approximates  Dijkstra  algorithm  performance,  approaching 
order  0(n)  steps  only  when  all  obstacles  are  between  the  start  point  and  the  goal. 

Thus  in  every  case  A*  search  algorithm  performance  is  superior  to  the  Dijkstra 
algorithm.  A  formal  proof  regarding  the  optimality  of  the  A*  search  method  in  graph 
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Figure  7.17  Challenging  circle  world  visibility  graph 
searching  can  be  found  in  (Hart  68). 

The  circle  sweep  algorithm  (Figures  7;10  and  7.11)  calculates  visibility  from  a 
point  or  circle  to  all  other  circles  in  order  O(nlogn)  complexity.  The  algorithmic 
complexity  of  the  complete  path  planning  problem  using  A*  search  is  therefore 
order  0(n2logn)  in  worst  case,  and  conjectured  to  be  order  0(nlog2«)  in  best  case.  It 
is  important  to  note  that  the  best  case  is  the  most  likely  to  occur  situation  where  all 
circles  are  distributed  randomly.  The  worst  case  is  the  less  likely  situation  where  all 
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circle  obstacles  lie  directlybetween  the  start  and  goal  points.  Additional  work  is; 
needed  io  formally  determihefhe  complexity  ofA*searchrelativeio:Dijkstrasearch 
given  the  special  geometric  constraints  of  a  geneM  circle  worldi 

F.  IMPLEMENTATION  AND  RESULTS 

The  author  has  written  a  program  in  ANSI  C  to  illustrate  and  evaluate  the 
methods  described  in  this  paper.  The  full  program  consists  of  geometric  primitive 
data  structure  definitions,  a  library  of  spatial  reasoning  functions,  tangent  visibility 
determination  routines,  search  functions  to  perform  either  Dijkstra  or  A*  search, 
graphical  screen  and  hard  copy  output,  and  a  text-based  user,  interface.  Source  code  is 
included  as  Appendix  F. 

Two  methods  are  employed  for  output  Primary  output  consists,  of  large 
coordinate  text  files  including- embedded,  labels  (Figures  7.15,  7.16  and‘7.17),  These 
files  can  be  sent  to  create  two-dimensional  graphical  output  (Figure  7.18)  using 
plotting,  routines  such  as  the  Unix  graph  command  or  the  sunplot  routine 
(Muliender  87).  Printed  hard  copy  or  screen  graph  output  can  be  generated. 

Secondary  output  consists  of  a  fisting  of  geometric  primitives  such  as  circles, 
points,  paths,  segments  and  arcs  generated  during,  execution.  These  high-level  text 
outputs  can  be  easily  used  as  input  for  revised  circle  world  search  rims  (Figure  7.19); 
The  outputs  can  also  be  passed  as  input  to  independent  robot  motion  control  processes 
or  graphical  simulation  programs  such  as  the  NPS  AUV  Integrated  Simulator. 

ANSI  C  was  chosen  as.the  project  programming  language  for  purposes  of 
efficiency,  portability  and  compatibility  with  other  robot  applications  at  NPS.  It  is 
important  to  note  that  circle  world  program  inputs  and  outputs  are  language  and 
platform  independent,  however.  While  the  circle  world  , programs  normally  run  on 
powerful  workstations  under  the  Unix  operating  system,  they  are  completely  portable 
to  other  architectures  such  as  the  IBM  PC  or  GESPAC  68030/OS-9  running  on  the 
NPS  AUV. 
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Figure  7.18  Excerpt  [from  graphics  plot  file  intermediate  output 


The  current  circle  world  implementation  program  uses  an  exhaustive  and 
iterative  visibility  determination  algorithm  instead  of  the  more  efficient  sweep  method 
algorithm  presented  ;here.  Source  code  optimization  is  worthwhile  but  not  mandatory 
prior  to  integration  on  board  an  operational  real-time  vehicle  such  as  the  NPS  AUV. 

G.  THREE-DIMENSIONAL  APPLICATIONS  AND  FUTURE  WORK 

The  two-dimensional  approach  to  path  planning  provided  by  the  circle  world 
model  can  be  directly  used  for  robot  motion  on  any  planar  surface,  such  as  a 
laboratory,  shop  or  warehouse  floor.  Interestingly,  the  circle  world  approaches  not 
constrained  to  path  planning  in  two  dimensions.  This  extendability  is  a  valuable  result 
since  efficient  path-planning  algorithms  in  three  dimensions  are  considered  to  be  an 


115 


Ciicle_World  Shortest  Path  Determination 


Point 

10.00  5.00 

0.00  Start 

Point 

125,00  65:00 

0.Q0  Goal 

Circle 

10.00  25.00 

0.00 

5.00 

Circle 

40.00  30.00 

0.00  20.00 

Circle 

75.00  20.00 

0.00  15.00 

Circle 

100.00  45.00 

0.00  18.00 

Circle 

20.00  60.00 

0.00  5.00 

Circle 

115.00  10.00 

0.00 

5.00 

Circle 

65.00  55.00 

0.00 

5.00 

Path  planning  results: 

Best  path  (cost  143.8) 

Segment 

5.00 

5.00 

0.00 

42.72  10.19 

0.00 

Arc 

40.00 

30.00 

0.00 

20.00  277.83 

328.11 

1=CCW 

Segment 

56.98 

19.43 

0.00 

62.26  27.92 

0.00 

Arc 

75.00 

20.00 

0.00 

15.00  148.11 

139.87 

-1=CW 

Segment 

63.53 

29.67 

0.00 

86:24  56.60 

0.00 

Arc* 

100.00 

45.00 

0:00 

18.00  139.87 

94.45 

-1=CW 

Segment 

98.60 

62.95 

0.00 

125.00  65.00 

0.00 

Figure  7.19  High-level  text  listing  of  example  NPS  pool  circle  world  and 
shortest  path  determination 


area  where  more  research  is  needed  (Yap  87). 

A  circle  world  can  be  used  for  robot  path  planning  across  irregular  land  terrain. 
The  fact  that  such  terrain  may  not  be  level  is  not  limiting  as  long  as  the  robot  can 
generally  traverse  it.  Only  vertical  obstructions,  interfering  holes  and  excessive  slope 
variations  that  prevent  safe  robot  passage  need  be  modeled  as  circle  obstacles. 
Remaining  terrain  surface  features  can  be  treated  as  planar  and  part  of  the 
obstacle-free  portion  of  the  configuration  space. 

Three-dimensional  path  planning  by  robot  vehicles  can  typically  be  performed 
using  a  similar  circle  world  approach.  Most  three-dimensional  obstacles  can  be 
represented  as  circles  by  taking  the  cross-section  of  each  obstacle  on  a  level  plane 
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used  for  robot  travel  (Figure  7.20).  An  irregular  objectcan  then  be  modeled  using  a 
vertical  cylinder.  Such  a  cylinder  defines  the  minimum  radius  circle  needed  to  enclose 
all  portions  of  the  object  which  are  collision  threats  in  the  plane  or  region  of  robot 
travel.  Underwater  obstacles  can  be  modeled  in  a  similar  fashion,  where  the  circle 
world  ground  plane  represents  the  allowable  depth-band  of  robot  submarine  travel. 

These  circle  world  conditions  are  necessary  and  sufficient  to  model  any  set  of 
real  world  obstacles  as  long  as  robot  motion  is  along  a  plane  or  within  a  planar  region. 
Three-dimensional  obstacles  are  represented  as  cylinders,  and  a  path  planning  search 
through  such  a  three-dimensional  obstacle  space  is  reduced  to  a  two-dimensional  circle 
world  search.  Such  a  representation  is  particularly  natural  for  underwater  obstacles. 

An  example  set  of  underwater  obstacles  in  the  NPS  pool  is  shown  with  a 
corresponding  shortest  path  in  Figure  7.21,  which  is  a  direct  output  of  the  circle  world 
path  planning  program.  The  same  data  set  is  also  shown  in  Figure  7.22  as  rendered 
by  the  NPS  AUV  Integrated  Simulator  using  high-level  text  output  from  the  circle 
world  path  planning  program. 

These  circle  world  methods  are  extendable  to  an  analogous  approach  for  polygon 
world  modeling.  Additional  program  coding  is  needed  to  address  overlapping  and 
adjacent  obstacles.  The  simplicity,  efficiency  and  effectiveness  of  the  circle  world 
model  makes  it  well  suited  for  real-time,  path  planning  by  mobile  robots. 
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Figure  7.20  Three-dimensional  cylindrical  obstacles  viewed  as 
two-dimensional  circles 
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Figure  7.22  Three-dimensional  representation  of  obstacles  in  the  NPS  pool 
H.  CONCLUSIONS 

Mobile  robot  path  planning  around  obstacles  can  be  accomplished  by  modeling 
obstacles  as  pairs  of  circles  with  opposite  rotations.  Addition  of  robot  radius  and 
safety  standoff  distances  to  circle  radii  allows  modeling  the  robot  as  a  point.  This 
circle  world  model  can  be  used  to  calculate  shortest  paths  between  points. 

Tangents  between  circles  in  a  circle  world  have  no  inherent  redundancy  or 
duplication  due  to  the  uniqueness  of  landing  and  leaving  points  on  each  circle. 
Alternate  partial  paths  landing  at  an  intermediate  circle  obstacle  must  be  properly 
compared  in  order  to  determine  which  is  shortest.  Dijkstra’s  algorithm  or  (preferably) 
A*  search  can  selectively  use  visibility  and  partial  path  comparison  calculations  to  find 
the  shortest,  safest  or  optimal  path  between  start  and  goal  points. 

Tangent  visibility  from  a  single  point  to  all  circles  can  be  calculated  in 
order  0(nlog«)  time.  Similarly,  tangent  visibility  from  a  single  circle  to  all  other 
circles  can  be  calculated  in  order  O(nlogn)  time.  The  shortest  path  between  start  and 
goal  points  can  be  calculated  in  order  0(n2logn)  time. 

Obstacle  avoidance  is  a  typical  robot  behavior  regardless  of  obstacle  height.  The 
circle  world  search  model  is  directly  extendable  to  the  general  case  of 
three-dimensional  path  planning  and  is  particularly  suitable  for  underwater  vehicle  path 
planning.  Future  circle  world  path  planning  implementations  on  a  robot  vehicle  should 
include  polygon  obstacles  as  well  as  overlapping  and  adjacent  obstacles. 


Vm.  REAL-TIME  OPERATING  SYSTEM  AND  AUV  SIMULATION 
CONSIDERATIONS 

A.  NPS  AUV  AND  REAL-TIME  OPERATIONS 

The  NPS  AUV  is  an  untethered  robot  submarine  designed  for  research  in 
adaptive  control,  mission  planning,  missior/execution,  and  post-mission  data  analysis 
(Healey  90).  AUVs  are  typical  of  other  autonomous  robots  in  that  a  large  number  of 
internal  processes  must  run  simultaneously  while  meeting  stringent  real-time 
requirements.  AUVs  differ  from  other  robots  in  that  they  are  designed  to  operate 
submerged  and  isolated  from  communication  or  external  directions.  Such  missions 
require  extraordinarily  reliable  and  robust  vehicle  performance. 

The  principles  and  concepts  particular  to  real-time  operating  systems  are  clearly 
defined  in  a  wide  variety  of  references  (Blackman  76)  (Mellichamp  83)  (Deitel  90) 
(Nelson  92).  An  explanation  of  real-time  control  issues  directly  relating  to 
autonomous  robot  vehicles  can  be  found  in  an  excellent  case  study  comparison 
between  the  Ohio  State  University  Adaptive  Suspension  Vehicle  (ASV)  and  the 
Defense  Advanced  Research  Projects  Agency  (DARPA)  Autonomous  Land  Vehicle 
(ALV)  (Payton  91). 

Although  numerous  software  modules  have  been  written  with  the  NPS  AUV  in 
mind,  very  little  software  has  actually  been  implemented,  integrated  or  tested 
underwater  in  real  time.  The  main  reason  for  this  deficiency  is  the  current  lack  of  a 
flexible  high-level  software  control  module  that  can  efficiently  coordinate  multiple 
NPS  AUV  processes  using  the  OS-9  real-time  operating  system. 

This  chapter  examines  the  real-time  operating  system  issues  that  are  pertinent  to 
the  development  of  the  NPS  AUV.  These  considerations  pertain  equally  to  the 
Gespac/OS-9  microprocessor  and  support  hardware  portions  of  the  NPS  AUV 
Integrated  Simulator. 
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B.  HARD  AND  SOFT  REAL-TIME  REQUIREMENTS 

In  order  to  perform  numerous  sophisticate  mission  functions;,  multiple  processes 
must  be  operating  simultaneously  while  meeting  both  strict  and  relaxed  real-time 
schedule  requirements.  The  autonomous  nature  of  an  AUV  requires  operation  without 
external  backup  in  a  harsh  and  unforgiving  environment  Vehicle  control,  sensor 
evaluation,  underwater  navigation,  search,  path  planning,  obstacle  avoidance,  fault 
tolerance,  and  numerous  other  processes  are  required.  All  processes  must  interact  with 
the  external  environment  and  each  other  in  real  time  with  varying  degrees  of 
interdependence  (Bobrow  91), 

It  is  important  to  distinguish  between  hard  and  soft  scheduling  criteria  for 
real-time  processes.  Mission-critical  actions  such  as  vehicle  control  and  failure 
detection  are  hard  real-time  scheduling  requirements.  Failure  to  meet  such  hard 
deadlines  may  result  in  mission  failure  or  even  catastrophic  loss  of  the  vehicle. 
Conversely,  high  level  logical  processes  such  as  path  planning  or  mission  replanning 
might  always  be  considered  soft  requirements,  since  their  execution  is  rarely 
mandatory  for  safe  vehicle  operation  and  immediate  results  are  not  required.  Finally, 
some  processes  may  have  priorities  that  vary  from  soft  to  hard  depending  on 
circumstances.  For  example,  obstacle  avoidance  is  typically  a  soft  requirement  until 
target  proximity  or  rapidly  closing  range  rate  make  immediate  action  necessary  to 
avoid  collision. 

C.  NPS  AUV  PROCESS  DEADLINE  SPECIFICATION  AND  SCHEDULING 

The  current  NPS  AUV  mission  software  schedule  runs  a  single  simple  mission 

control  loop  using  a  10  Hz  clock.  A  full  100  millisecond  interval  is  allotted  for  each 
mission  loop,  but  no  processes  are  allowed  to  exceed  that  period.  This  interval  is 
adequate  to  perform  numerous  tasks:  compute  basic  vehicle  control  orders,  transmit 
using  one  to  four  sonar  transducers,  record  all  current  vehicle  data  parameters  in 
working  memory,  perform  rudimentary  sonar  analysis,  detect  waypoints,  detect 
potential  collision,  and  order  predetermined  state  changes  in  propeller  speed  and 
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control  surface  position.  Typically  very  little  time  remains  at  the  end  of  each  fixed 
100  millisecond  time  segment.  Such  a  simple  hard-wired  timing  mechanism  is  not  a 
feasible  control  architecture  for  AUV  software  of  even  slightly  greater  complexity. 

Proposed  NPS  AUV  software  modules  are  shown  by  the  data  flow  diagram  of 
Figure  8.1  (Healey  90).  Only  the  basic  interdependencies  of  these  NPS  AUV  task 
modules  have  been  characterized.  A  formal  analysis  of  software  module 
specifications,  timing  requirements,  task  periodicities  and  concurrency  dependencies 
has  yet  to  be  performed.  It  is  likely  that  the  NPS  AUV  software  modules  will 
ultimately  have  timing  constraints  and  periodicity  characteristics  similar  to  those 
developed  in  Table  VIII.  1. 


Current  real-time  operating  system  research  at  NPS  has  focused  on 
rate-monotonic  scheduling  theory,  an  approach  that  employs  fixed  prioritization  of 
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Table  VHU  AUV  SOFTWARE  MODULE  REAL-TIME 
CHARACTERISTICS 


AUV  Software  Modules 

Real-Time.  Execution  Characteristics 

Plan/Replan  Mission 

M 

Aperiodic 

Event  triggered 

S 

Execute  Mission 

Hard 

Periodic 

' 

Every 

control 

loop 

Guidance 

Both 

Periodic 

Every  loop 

unless 

preempted 

Autopilot 

Hard 

Periodic 

Every 

control 

loop 

Process  Sonar  Data 

Periodic 

Every  loop 

unless 

preempted 

Navigate 

Soft 
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Every  loop 

unless 

preempted 

Monitor  Systems  Status 

Both 

Periodic 

Every  loop 

unless 

preempted 

Avoid  Obstacles 

Hard 

Periodic 

Every 

control 

loop 

processes  and  guarantees  acceptable  average  performance  (Leatherman  91) 

(Makris  91).  Benefits  of  rate-monotonic  scheduling  include  guaranteed  completion  of 
periodic  tasks  in  order  of  priority,  fast  response  for  aperiodic  tasks,  modifiable  task 
priorities,  and  the  scheduling  of  tasks  that  permit  imprecise  computations  (i.e.  output 
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precision  proportional  to  time  available).  Rate-monotonic  task  analysis  and  scheduling 
is  performed  off-line  prior  to  actual  execution  of  system  software.  Rate-monotonic 
scheduling  implementations  running  dummy  processes  under  the  QS-9  operating 
systemshave  been  shown  to  provide  processor  utilization  above  80%  . and  graceful 
degradation  under  overload; 

Dynamic  adjustment  of  constraints  and  process  schedules  may  ultimately  be 
required  to  ensure  successful AUV  operation  during  unforeseen  tactical  scenarios  or 
pathological  process  conflicts.  Dynamic  scheduling  theory  requires  further  formal 
research  to  provide  a  verifiable  theoretical  foundation,  but  it  appears  to  be  a  desirable 
model  for  the  distributed  artificial  intelligence  applications  likely  to  make  up  the 
AUV.  In  this  regard  the  FLEX  programming  language  is  worth  consideration  since  it 
implements  dynamic  scheduling  theory  and  generates  C++  code  as  output  (Kenny  91). 
Additionally  there  exists  a  hybrid  approach  known  as  a  mixed  priority  system  that 
combines  the  best  features  of  rate-monotonic  scheduling  and  dynamic  scheduling 
(Leaiherman  91).  Formal  evaluation  of  the  mixed  priority  approach  also  appears 
worthwhile. 

Given  that  AUV-related  research  is  likely  to  continue  for  many  years  by  several 
academic  departments  at  NPS,  operational  software  changes  and  additional  new 
software  processes  will  always  be  under  development  and  require  integration  into  the 
overall  NPS  AUV  system  process  schedule.  Reliability,  compatibility  and 
extendibility  for  future  growth  must  be  key  requirements  for  any  proposed  control 
process  timing  schedule.  Robust  and  flexible  interactions  between  numerous 
interdependent  processes  will  be  essential  to  allow  frequent  improvements  to  vehicle 
performance  while  maintaining  vehicle  reliability. 

D.  PARALLEL  PROCESSING  AND  CONCURRENT  PROGRAMMING 

It  is  important  to  note  that  parallelism  is  equally  as  important  as  real-time 
scheduling  for  an  AUV  operating  system.  This  is  particularly  true  if  low-level  control, 
complex  behaviors,  sensor  fusion,  data  analysis,  mission  planning  and  numerous  other 
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artificial,  iritelligehce  aspects  of  robot  mission  execution  must  allcoexist  andcooperate 
in  a  rapid  manner  (Stankovic  88). 

Non-trivial  robot  performance  requires  that  numerous  processes  operate  in 
parallel,  either  independently  or  in  a  mutually  dependent  fashion  (Kasahaia  88). 
Numerous  challenging  AUV  mission  requirements  will  inevitably  lead  to  multiple 
software  modules  operating  concurrently;  Such  parallelism  might  be  most  easily 
implemented  using  a  multiprocessor  architecture.  An  AUV  -  s  real-time  operating 
system  must  completely  support  concurrency  constructs  that  are  fully  integrated  with 
the  real-time  scheduling  mechanisms. 

Several  standard  features  of  parallel  programming  are  necessary  for  effective 
software  engineering  of  a  real-time  AUV.  Adequate  shared  memory  is  essential  if 
numerous  processes  are  to  quickly  and  efficiently  access  system  state  variables  and  the 
large  amounts  of  time-sensitive  sensor  data  that  is  expected.  Predictable  rendezvous, 
synchronization  and  communication  methods  must  be  available,  both  for  interaction 
between  mutually  dependent  processes  as  well  as  loose  overall  control  by  a  mission 
executor  module. 

Increasing  hardware  sophistication  can  further  allow  tasks  to  be  distributed  over 
a  network  among  separate  specialized  embedded  processors  (Stankovic  88). 

Extensions  of  process  rendezvous,  synchronization  and  communication  must  be 
available  for  distributed  processing  if  networked' processors  are  to  be  employed. 

Massively  parallel  processing  in  the  classic  sense  uses  numerous  processors  in 
parallel  to  perform  array  processing  or  numerous  parallel  solutions  of  identical 
algorithms.  Such  an  approach  is  not  a  likely  requirement  for  AUV  operation.  Aside 
from  potential  analysis  of  sophisticated  sensor-data  using  vision  processing  techniques* 
few  (if  any)  AUV  functions  can  be  decomposed  into  numerous  identical  subproblems. 
The  diverse  nature  of  the  many  AUV  software  modules  implies  that  a  transputer 
architecture  is  not  a  prerequisite  for  successful  integration  of  multiple  AUV  processes. 
Nevertheless  the  transputer  paradigm  may  be  an  effective  way  to  minimize  interface 
difficulties  while  allowing  unlimited  addition  of  numerous  unique  parallel  processes 
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under  a  single,  irite^ated  redTtime^opefating  .system.  This  approach  is  also  being 
considered  by'C.S.  Draper  Laboratories  for  the  next-generation  software  architecture  of 
the  DARPA  Unmanned  U nderwater  Vehicle  (UUV)  (Hale9 1 ). 

E.  OPERATING  SYSTEM  COMPATIBILITY  AND  INTEROPERABILITY 

It  is  important  that  the  AUV  operating  system  be  fully  compatible  with  all 
current  and  projected  vehicle  hardware  and  software.  External  connectivity  of  the 
real-time  operating  system  is  also  important. 

Hardware  interoperability  considerations  must  consider  connections  between 
multiple  processors  of  various  types  internal  to  the  vehicle,  as  well  as  numerous 
analog/digital  and  digital/analog  interfaces.  Space,  weight  and  power  requirements  are 
very  strict  so  internal  AUV  hardware  architectures  must  be  closely  compatible. 
Physical  compatibility  improves  vehicle  endurance  by  reducing  power  consumption. 

Software  compatibility  is  less  critical  than  hardware  compatibility,  but  software 
incompatibilities  can  still  impose  undesirable  processing  delays  if  too  much  work  is 
required  to  translate  communications  between  processors.  Network  support  and 
software  interfacing  will  be  needed  when  different  operating  systems  reside  on 
multiple  processors.  Multiple  programming  language  support  is  desirable  for 
unrestricted  research  in  a  variety  of  control  system  and  artificial  intelligence  subjects. 
Process  encapsulation  is  desirable  in  order  to  minimize  faults  and  side  effects  during 
software  development  High-level  software  access  to  machine-dependent, 
machine-level  and  device-dependent  routines  is  also  needed.  Such  routines  permit 
various  processes  to  utilize  the  operating  system  for  direct  access  and  control  of  the 
numerous  physical  components  of  the  AUV. 

Although  an  AUV  is  untethered  and  isolated  during  operation,  a  number  of 
external  compatibility  requirements  remain.  Mission  data  collection,  consolidation, 
storage  and  transmission  are  ultimately  targeted  for  external  off-line  post-processing 
and  analysis.  Distributed  processing  over  a  network  internal  to  the  AUV  requires  that 
each  individual  operating  system  must  be  able  to  interact  with  the  others.  Interactive 
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network  communication  is  also  a  likely  requirement  for  on-line  laboratpry  testing  of 
the  AUV.  Connecting  the  vehicle  or  similar  lab  prototypes  to  ari  integrated  simulator 
allows  scientific  visualization^  of  AUV  processes  for  active  real-time  end-to-end 
developmental  testing.  For  these  reasons  the  AUV  must  be  able  to  communicate  in 
some  fashion  with  non-native  operating  systems  and  software  environments.  External 
connectivity  is  essential  to  support  the  diverse  and  distributed  communities  that 
conduct  AUV  research. 

Several  other  operating  systems  are  worth  noting.  Real-time  constructs  and 
compatibility  can  be  incorporated  into  typically  non-real-time  operating  systems  such 
as  Unix  by  adding  specially  designed  message-passing  processes  (Cramer  88) 

(Falk  88)  (Hildebrand  88).  Modified  mal-time  kernels  of  common  operating  systems 
such  as  Modular  Computer  Systems  Inc.’s  Real/IX  for  Unix  or  Digital  Research  Inc.’s 
FlexOs  iox  DOS  are  viable  and  commercially  available  (Falk  88)  (Baerson  91). 
Standards  development  work  continues  for  Posix,  an  open  operating  system 
specification  based  on  Unix  that  includes  real-time  constructs  (Deitel  90)  (Falk  88). 

As  robotic  systems  and  intelligent  machines  become  more  commonplace,  the 
interactive  design  concepts  of  TRON  (The  Real-time  Operating  system  Nucleus)  will 
become  increasingly  important  (Kahaner  91).  Finally,  a  distributed  operating  system 
may  provide  the  most  efficient  control  mechanisms  for  distributed  processors  sharing 
distributed  resources  (Dasgupta  91). 

F.  OS-9  OPERATING  SYSTEM 

OS-9  is  Microware  System  Corporation’s  real-time  operating  system  used  by  the 
NPS  AUV.  OS-9  is  designed  to  run  exclusively  on  the  Motorola  68020/68030/68040 
family  of  microprocessors  (GESPAC  89).  the  two  specific  hardware  configurations 
used  in  the  NPS  AUV  include  GESPAC  68020  or  68030  microprocessors  connected  to 
a  GESBUS  (VME  bus  compatible)  backplane.  Serial  ports,  parallel  ports, 
analog/digital  interface  cards  and  an  Ethernet  interface  are  available  for  internal  and 
external  connections.  Also  available  for  internal  networking  is  an  Intel  80386 
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microprocessor.  INMOS  T805/T425  transputers  were  also  to  be  cpnriecfed  but  are  no 
longer  manufactured  in  a  configuration  compatible  with  the  GESBUS.  No  suitable 
transputer  replacement  has  yet  been  identified. 


The  OS-9  process  states  available  include  start  (fork),  active,  run,  exit,  sleep, 
wait  (process  synchronization),  and  event  wait  (semaphore  communication).  Process 
state  transitions  are  shown  iii  Figure  8.2  (GESPAC  89). 


OS-9  features  that  support  expected  AUV  operating  system  requirements  include 
adjustable  priorities  and  aging  for  explicit  execution  scheduling,  preemptive  process 
switching  based  on  priority,  reprogrammable  interrupts,  a  trap  library,  events  for 
process  synchronization,  signal  communication  between  processes,  pipes  for 
interprocess  data  transfer,  and  redirection  of  process  inputs  and  outputs  (Dibble  88). 
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Identical  syntax  when  referring  to  processes  or  device  drivers  is  a  particularly 
convenient  feature  of  OS-9. 

Deficiencies  and  shortcomings  of  OS-9  include  the  current  lack  of  compatible 
Ada  or  C++  compilers  and  no  simple  method  of  deadlock  protection.  Additionally  a 
fully  modifiable  operating  system  kernel  must  be  prepared  through  careful  EEPROM 
configuration  prior  to  operation.  This  preparation  allows  setting  up  the  operating 
system  to  include  only  the  device  drivers  that  are  necessary  for  the  current  vehicle 
hardware.  OS-9  is  very  flexible  in  that  additional  drivers  may  be  loaded  at  any  time 
after  system  initialization  by  software  command.  However  the  EEPROM 
configuration  process  is  time  consuming,  version  dependent  and  error  prone  due  to 
limited  self-diagnostic  testing. 

G.  CURRENT  PROBLEM  AREAS  AND  FUTURE  RESEARCH 

Deadlock  detection  in  a  real-time  vehicle  can  be  guaranteed  by  designing  a 
special  periodic  real-time  process  for  that  purpose.  An  example  can  be  shown  using 
the  NPS  AUV  software  module  information  in  Figure  8.1  and  Table  VIII.I.  The 
NPS  AUV  has  a  tight  inner  control  loop  that  includes  the  Mission  Executor  and 
Autopilot  that  must  completely  repeat  on  a.  frequent  periodic  basis  of  approximately 
one  second.  These  two  periodic  processes  can  be  required  to  toggle  state  variables 
every  time  the  one-second  control  cycle  is  successfully  completed.  Failure  to  do  so 
after  several  seconds  is  a  clear  indication  of  some  type  of  critical  problem  such  as 
deadlock.  Recovery  after  deadlock  detection  can  be  promptly  accomplished  by 
reinitializing  vehicle  control  loop  software.  This  new  approach  to  real-time  deadlock 
detection  is  a  straightforward  solution  to  a  problem  that  is  frequently  considered 
intractable  in  non-real-time  operating  systems. 

Deadlock  prevention  is  expensive  but  essential  because  the  independent, 
unmonitored  and  uncontrolled  nature  of  an  AUV  makes  reliability  paramount  for 
vehicle  survivability.  Redundant  approaches  to  deadlock  prevention,  deadlock 
detection  and  deadlock  recovery  are  worthwhile.  Resolution  of  deadlock  is  a 
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particularly  sensitive  area,  given  the  frequently  changing  NPS  AUV  software  and  the 
unpredictable  ordering  of  process  preemptions  and  interactions  in  real  time. 

As  various  software  modules  are.  integrated  into  the  NPS  AUV,  software 
engineering  considerations  become  increasingly  important  Key  issues  are;  systems 
integration,  verification  and  validation  of  process  behaviors  despite  real-time 
interaction  uncertainties,  software  version  control,  and  system  software  upward 
compatibility  for  integration  of  future  software  modules,  Failure  to  methodically 
address  software  engineering  issues  will  undoubtedly  lead  to  unpredictable  AUV 
behavior  and  tremendous  amounts  of  time  wasted  troubleshooting  individual  software 
modules  rather  than  subtle  faults  in  the  operating  system  implementation. 

Fault  tolerance  is  also  needed  to  guarantee  overall  vehicle  reliability  and 
robustness.  The  approach  taken  needs  to  primarily  rely  on  software  checks,  rather 
than  the  use  of  redundant  processors  found  in  some  larger  vehicles  (Hale  9i).  Fault 
tolerance  requirements  will: need  to  be  specified  for  the. top-level  mission  executor  as 
well  as  all  individual  processes.  Selection  of  a  distributed  multiprocessor  architecture 
allows  hardware-based  fault  tolerance,  since  failure  of  a  given  node,  can  be 
functionally  corrected  by  reloading  and  sharing  the  lost  software  modules  on  the 
remaining  working  processors. 

Further  work  is  needed  to  define  formal  specifications,  characteristics  and  timing 
constraints  for  all  NPS  AUV  software  modules.  Software  module  specifications  need 
to  include  inputs  and  outputs,  functionality,  module  dependencies,  hard  or  soft 
scheduling  constraints,  periodic  or  aperiodic  execution,  relative  priorities,  expected 
frequency  and  duration,  and  all  other  parameters  of  importance  to  integrated  system 
design. 

The  top  priority  for  NPS  AUV  operating  system  software  integration  is  to 
establish  a  new  baseline  architecture  of  system  software  running  in  the  vehicle,  this 
will  allow  more  sophisticated  operations  and  the  addition  of  new  processes  to  the  basic 
control  loop.  It  is  unfortunate  that  most  theses  written  about  the  NPS  AUV  to  date 
have  been  unable  to  test  their,  conclusions  using  the  actual  vehicle  in  the  water. 


131 


Ensuring  maximum  processor  utilization  through  improvements  to  the 
rate-monotonic  scheduling  algorithm  is  important  work  that  is  expected  to  continueby 
verifying  current  scheduling  conclusions  using  actual  NPS  AUV  processes.  The 
incorporation.of  dynamic  scheduling  features  holds  great  promise  for  the  effective 
coordination  of  numerous  distributed  artificial  intelligence  software  modules. 

Perhaps  the  most  interesting  research  immediately  applicable  to  the  NPS  AUV  is 
the  investigation  of  alternate  system  software  architecture  organizations;  Many 
possibilities  are  available  which  might  incorporate  multiple  intelligent  agents, 
low-level  behaviors,  expert  systems  and  blackboard  paradigms  (Wright  88) 

(Durfee  88).  A  real-time  architecture  that  allows  flexible  support  of  a  variety  of 
compatible  software  approaches  will  provide  the  best  framework  for  rapid  research 
progress. 

The  NPS  AUV  is  a  key  project  that  integrates  many  of  the  critical  technologies 
important  to  the.  Navy  of.  tomorrow.  The  successfuliestablishment  of  a  reliable  and 
robust  real-time  system  software  architecture  will  be  the  foundation  that  supports  all 
future  NPS  AUV  operations. 
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EC  PERFORMANCE  EVALUATION  ANDFUTURE  RESEARCH 

A.  SIMULATOR  LIMITATIONS  AND  PERFORMANCE  MEASUREMENTS 

The  NPS  AUV  Integrated  Simulator  is  designed  to  eliminate  as  many  design 
restrictions  as  possible  in  a  distributed  research  environment.  There  are  two  primary 
limitations  that  restrict  integrated  simulator  performance:  graphics  simulation  program 
display  rate  and  data  transfer  rates. 

The  NPS  AUV  Integrated  Simulator  graphics  simulation  program  is  written  in 
ANSI  C  and  uses  the  GL  Graphics  Library  torun  on  a  Silicon  Graphics?Inc. 

Iris/4D  240VGX  workstation.  The  complexity  or;  drawing  panel  interfaces  and 
multiple  complex  objects  restricts  the  playback  speed  the; graphics  simulation;prograrn 
can  maintain.  Maximum  speed  of  playback  with  no  associated  environmental  objects 
to  be  drawn  is  7  Hz  (i.e.  7  frames  per  second),  nearly  matching  the  telemetry  sampling 
rate  of  10  Hz.  Worst  case  playback  screen  update  rate  using  actual  in-water  test  data: 
has  been  1-2  Hz.  Playback  rates  are  correspondingly  lower  on,  less  capable  Iris 
workstations.  Although  the  worst  case  1-2  Hz  screen  update  rate  may  appear 
somewhat  jerky  to  the  user,  screen  vehicle  motion  accurately  renders  real-time  vehicle 
motion  since  intermediate  telemetry  data  records  are  skipped.  Further  improvements 
in  the  screen  update  rate  are  possible  if  control  panel  interface  code  is  optimized  and 
the  overall  simulation  program  is  tuned  and  parallelized  for  peak,  performance. 

Data  transfer  rates  over  the  network  currently  do  not  impact  integrated  simulator 
performance  since  data  packets  and  socket  software  are  not  yet  implemented. 

However  this  can  be  a  significant  bottleneck  that  prevents  realistic  performance,  given 
current  experience  with  packet-passing  simulation  (Byrnes  92).  Integrated  simulator 
implementation  of  software  sockets  must  be  tolerant  of  packet  delivery  time  delays  or 
nondeterministic  and  incorrect  results  will  occur. 
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B.  INTEGRATED  SIMULATOR  FOLLOW-ON  WORK 

Several  possibilities  invite  immediate  follow-oii  work  to  the  NPS  AUV 
Integrated  Simulator. 

A  vehicle  hardware  model  and  hydrodynamic  vehicle  response  model  need  to  be 
added  to  the  simulator  to  provide  realistic  simulation  of  vehicle  physical  response 
using  the  laboratory  AUV.  Previous  hydrodynamic  response  models  have  been  part  of 
the  graphics  simulation  code.  However  network  time  delays  do  not  allow  accurate 
response  or  correct  interaction  between  the  laboratory  AUV  and  the  graphics 
workstation.  Realistic  integration  of  hydrodynamic  response  with  simulation  can  be 
accomplished  by  making  the  model  an  independent  OS-9  process  or  placing  the  model 
on  a  separate  connected  microprocessor  inside  the  laboratory  AUV  backplane  chassis. 
Validation  of  the  model  can  be  conducted  through  test  comparison  with  actual  in-water 
data. 

Software  sockets  that  allow  passing  data  packets  between  processes  need  to  be 
implemented  in  a  way  that  is  simple  for  any  NPS  AUV  programmer  to  use.  Socket 
implementations  are  already  available  for  Unix  processes,  including  GL-based  graphics 
simulation  programs.  The  key  challenge  will  be  to  implement  compatible  software 
sockets  for  the  NPS  AUV  source  code  running  under  the  OS-9  operating  system. 

Sonar  visualization  capabilities  have  not  been  added  to  the  graphics  simulation 
program.  Addition  of  graphics  polygons  to  represent  sonar  beams,  echo  contact  points 
and  error  boundaries  will  improve  the  user’s  ability  to  visualize  real-time  interactions 
between  the  vehicle  and  the  environment. 

C.  POTENTIAL  FUTURE  RESEARCH 

The  NPS  AUV  Integrated  Simulator  provides  a  foundation  for  many  types  of 
future  AUV  research.  The  following  areas  are  of  particular  interest. 

The  NPS  AUV  vehicle  control  software  needs  to  allow  modular  addition  of 
software  processes  shown  in  the  block  diagram  of  Figure  2.6.  Current  vehicle 
software  is  limited  and  supports  only  rudimentary  behaviors.  Access  to  the 
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laboratory  AUV  via  the  integrated,  simulator  data  network  allows  all,  interested 
researchers  the  opportunity  to  test  their  programs  on  the  vehicle.  The  baseline  AUV 
control  software  must  be  upgraded  to  support  these  increased  demands. 

Scientific  visualization  techniques  hold  great  promise  for  rapid  understanding  of 
complex  physical  processes.  Visualization  can  be  used  for  comparison  of  theoretical 
and  empirical  data.  Close  evaluation  of  hydrodynamics  and  vehicle  sideslip  models 
may  reveal  general  techniques  for  formal  model  verification. 

Visualization  of  sonar  and  acoustic  interactions  is  a  promising  area  of  research. 
Sonar  visualization  can  be  directly  implemented  in  the  integrated  simulator  for  new 
and  proposed  sonar  types.  Sonar  visualization  will  greatly  increase  user  understanding 
of  sonar  performance  and  is  likely  to  have  tremendous  tactical  and  training 
significance. 

Simulation  of  world  models  is  not  a  precisely  defined  science.  Accurate 
simulation  world  models  are  needed  for  navigation,  the  external  environment, 
hydrodynamic  response,  sonar  acoustic  behavior  and  physical  vehicle  hardware 
components.  Additional  research  is  needed  to  determine  the  specifications  of  these 
world  models,  validate  their  correctness  and  show  how  best  to  implement  them  in  the 
context  of  an  integrated  simulator.  Successful  integration  of  general  world  models 
into  a  real-time  simulator  is  a  prerequisite  to  production  of  a  virtual  world  where 
complete  and  realistic  interaction  is  possible. 

Numerous  additional  research  examples  are  conceivable.  Current  and  future 
researchers  working  on  the  NPS  AUV  project  will  undoubtedly  develop  their  own 
applications  and  extensions  using  the  NPS  AUV  Integrated  Simulator. 
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X.  SUMMARY 


The  development  and  testing.of  AUV  hardware  and  software  is  greatly 
complicated  by  vehicle  inaccessibility  during  operation;  Integrated  simulation 
remotely  links  vehicle  components  and  support  equipment  with  graphics  simulation 
workstations.  Integrated  simulation  allows  complete  real-time,  pre-mission, 
pseudo-mission  and  post-mission  visualization  and  analysis  in  the  lab  environment. 
Integrated  simulator  testing  of  software  and  hardware  is  a  broad  and  versatile  method 
that  supports  rapid  and  robust  diagnosis  and  correction  of  system  faults. 

In  order  to  fully  understand  the  simulation  requirements  of  demanding  artificial 
intelligence  processes  necessary  for  AUV  operation,  in-depth  studies  are  included  for 
path  planning,  expert  system  sonar  classification  and  real-time  operating  system 
considerations.  Conclusions  specific  to  these  areas  of  research  are  included  with  each 
chapter. 

High-resolution  three-dimensional  graphics  workstations  can  provide  real-time 
representations  of  vehicle  dynamics,  control  system  behavior,  mission  execution, 
sensor  processing  and  object  classification.  The  flexibility  and  versatility  provided  by 
this  approach  enables  visualization  and  analysis  of  all  aspects  of  AUV  development. 
Integrated  simulator  networking  is  recommended  as  a  fundamental  requirement  for 
AUV  research  and  deployment.  The  availability  of  the  NPS  AUV  Integrated 
Simulator  for  distributed  research  promises  to  benefit  all  future  NPS  AUV  work. 
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APPENDIX  A.  NPS  AUV  INTEGRATED  SIMULATOR  USER’S  GUIDE 

The  NPS  AUV  Integrated  Simulator  is  designed  to  be  accessible  to  anyone 
performing  AUV-related  work  at  NPS.  This  User’s  Guide  shows  how  to  utilize  the 
integrated  simulator  for  mission  playback,  software  design  or  vehicle  visualization. 

An  additional  example  which  shows  how  a  standalone  application  can  use  the  graphics 
simulation  program  to  visualize  NPS  AUV  behavior  can  be  found  in  (Compton  92). 

1.  NPS  AUV  GRAPHICS  SIMULATION  EXECUTION 

The  graphics  simulator  program  is  executed  by  logging  on  one  of  the  Iris 
workstations,  changing  to  the  appropriate  directory  (default  ~brutzman/auv)  and  typing 
auvsim.  The  auvsim  graphics  simulation  program  loads  all  high-level  objects  and 
commands  in  the  current  pool.auv  file.  Users  can  move  their  viewpoint  and  reference 
point  around  the  simulated  world,  run  telemetry  replay  files  and  reposition  individual 
objects  as  desired. 

2.  NPS  AUV  INTEGRATED  SIMULATOR  CONTROL  PANEL 

A  simple  control  panel  has  been  provided  as  the  user  interface  to  the  simulator. 
Two  types  of  files  may  be  entered  in  the  filename  type-in  box:  "filename.auv" 
high-level  object  files  and  "filename.d"  telemetry  files.  High-level  object  files  are 
used  to  enter  objects  into  the  simulated  world,  position  the  AUV,  change  the 
background  environment  graphics  object  and  provide  commands  to  the  graphics 
simulation  program.  Telemetry  replay  files  are  files  of  floating  point  records,  each  of 
which  represents  NPS  AUV  state  at  a  given  time.  File  specifications  are  described  in 
detail  in  Chapter  HI.  Sample  high-level  object  files  and  telemetry  replay  files  are 
shown  in  Figures  A.l  and  A.2. 

A  great  deal  of  functionality  is  included  on  the  control  panel.  The  name  and 
number  of  the  current  object  of  interest  is  displayed  along  with  position,  scale,  posture 
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Figure  A.1  Example  high-level  object  file 
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0.000000  0.000000  0.000000  0.161064  -0.054569  6.050224  0.056758  -0.133328 
-0.017602  -0.631889  6:125950  -6.045393  0  0  313  1  1558 
6.100000  0.199678  6.01 1346  0.158827  -0.078823  0.062779  6.057525  -0.155013 
-0.017602  -0.024220  0.131702  0.001013  0  0  308  1  1556 
0.200000  0.399347  0.022844  0.149879  -0.072759  0.071150  0.057909  -0.192962 
-0.030343  -0.029333  0.130105  0.008301  0  0  303  1  1556 
0.300000  0.599012  0.034419  0.147642  -0.057601  0.054409  0.057525  -6.133328 
-0.027795  -6.033168  0.127229  -0.041462  6  0  288  1  1554 
0.400000  0.798681  0.045918  0.140931  -0.063664  0.058594  0.056375  -0.117064 
-0.031617  -0.034446  0.123713  4X028987  0  0  285  1  1551 
0.500000  0.998363  0.057187  0.138694  -6.042443  0.050224  0.055224  -0.100800 
-0.037988  -0.044672  6. 1 15724  -0.071750  0  0  277  1  1551 
0.600000  1.198058  0.068226  0.143168  -0.048506  0.048131  0.055224  -0.062850 
-6.037988  -0.037002  6.119559  -0.083995  0  6  280  1  1552 
0.700000  1.397754  0.079265  0.147642  -0.063664  0.060687  0.055608  -0.095378 
-0.049454  -0.031889  0.123074  -0.073638  0  0  274  1  1555 
0.800000  1.597444  0.090381  0.154353  -0.051538  0.056502  0.056758  -0.122485 
-0.058373  -0.034446  0.124672  -0.H7965  0  0  276  1  1553 
0.900000  1.797122  6.101727  0.161064 -0.051538  0.048131  0.058676  -0.127906 
-0.053277  -0.029333  0.132022  -0.141904  0  0  276  1  1559 
1.000000  1.996778  0.113455  0.163301  -0.066696  0.050224  0.060210  -0.138749 
-0.040536  -0.017829  0.141609  -0.105188  0  0  268  1  1558 


Figure  A.2  One  second  excerpt  of  10  Hz  telemetry  replay  file 


and  time  tag.  Color  objects  can  also  be  displayed.  Color  objects  are  a  special  object 
type  which  control  the  RGB  (red  green  blue)  values  of  geometric  objects  such  as  lines 
and  walls  which  follow.  Operator  viewpoint  and  reference  point  control  are  provided 
through  dials  and  sliders  for  Height  between  viewpoint  and  reference  point,  Range  in 
the  x-y  direction,  Azimuth  between  viewpoint  and  reference  point  in  the  x-y  plane,  and 
viewpoint  Twist  angle.  With  a  little  practice  users  can  move  with  ease  throughout  the 
environment. 

Control  panel  function  buttons  are  also  provided.  *  The  Sonar  button  toggles  a 
plot  display  of  sonar  data  versus  time  for  left,  right,  forward  and  depth  transducers. 
Reset  returns  the  screen  to  its  initial  settings.  Snapshot  takes  a  black  and  white  picture 
of  the  pool  and  allows  the  user  to  then  select  a  small  portion  to  be  saved  as  a  figure  in 
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Silicon  Graphics  Inc.  rgb  format  ("snapshot.rgb")  and  Encapsulated  Postscript  format 
("snapshoteps").  The  Snapshot  feature  is  very  handy  for  creating  figures  to  be 
imported  by  Frame  or  WordPerfect,  but  graphics  tiles  are  very  large  so  this  feature 
should  be  used  sparingly.  More  information  on  snapshot  execution  can  be  found  in 
the  shell  script  "screensnapshot".  Replay  executes  or  resumes  the  current  telemetry 
replay  file,  and  Step  single  steps  through  the  telemetry  replay  file  one  record  at  a  time. 
Telemetry  data  may  be  viewed  by  selecting  the  AUV  object  or  popping  up  the  Unix 
command  line  window  over  the  screen  display.  Exit  quits  the  simulation  program. 
Buttons  are  selected  by  positioning  the  mouse  cursor  and  holding  down  the  left  mouse 
button.  Button  response  is  indicated  by  a  button  color  change. 

The  right  mouse  button  selects  a  multiple  level  menu.  Menu  control  is  provided 
for  viewpoint,  reference  point,  object  position,  rotation  and  scaling,  and  special 
simulator  features  such  as  lighting  model,  real-time  playback  toggle  and  system  usage 
performance  meter. 

The  dials  and  button  box  are  also  operative.  Buttons  1-2-3  and  5-6-7  move 
viewpoint  x-y-z  coordinates  towards  or  away  from  the  reference  point  respectively. 
Button  4  is  Reset  and  button  8  is  Exit.  Dial  6  selects  the  next  or  previous  object. 

Dials  4-2-0  pan  viewpoint  and  reference  point  together  in  x-y-z  directions  respectively. 
Dials  7-5 -3-1  are  Height ,  Range,  Azimuth  and  Twist  respectively.  Dial  and  button 
physical  configurations  are  shown  in  Figure  A, 3. 

3.  LABORATORY  GESPAC  EXECUTION 

NPS  AUV  control  loop  software  can  be  compiled  and  executed  on  network 
node  auvsiml,  the  laboratory  Gespac  version  of  the  NPS  AUV  microprocessor. 
Figures  A.4  through  A.8  show  a  sample  logon,  compilation  and  execution  of 
NPS  AUV  control  software  program  loop.c.  Following  execution  the  resulting 
telemetry  replay  file  "d.d"  is  transferred  to  a  graphics  workstation  where  it  can  be 
replayed  by  the  graphics  simulation  program  auvsim. 
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Figure  A.3 


NFS  AUV 


ile  auvsim.log: 


sample  execution  of  AUV- software  on  x 
iris  Lab- AUV- .Gespac  running  OS-9-  on  '9  JAN  9.1 


emini:/n/gemini/w6rk/brutzman>>  telnet  auvsiml 

rying  131.120.1.40  ... 

onnected  to  auvsiml.cs.nps.navy.mil. 

scape  character  is 

DS-9/68K  V2.3  Gescomp  84xx/86xx  -  68020  92/01/09  00:32:05 

Jser  name?:  brutzman 
Password: 

Process  #06  logged  on  92/01/09  00:32:13 
Welcome! 

******  WELCOME  TO  PROFESSIONAL  OS-9/ 68k  V2.3  ****** 

*  * 

*  S  C  /HO  =  Hard  Disk  #1  (20  Mb)  * 

*  0  * 

*  Y  N  /DO  Floppy  Disk  #1  (FDC-3)  * 

*  p  * 

*  S  I  * 

*  G  * 

*  T  U  /TERM  =  RS-232  Interface  to  Terminal  * 

*  R  * 

auvsiml  *  E  A  /T1  =  Auxiliary  Serial  Port  #1  * 

*  1<  * 

NPS  AUV  *  M  I  (/P  =  Centronics-type  Printer]  * 

*  0  * 
Integrated  *  N  * 

*  * 

Simulator  ******************************************************* 


S  dir 


ARC 

C 

INET 

MIT 

Release. notes 

SUBMODS 

d.d 

loop 

readme. utils 
startup. bak 
startup. ok 


AUV 

CEE 

INSTALL 

MSDOS 

SYS 

dir .fix 
loop.c 
romlist.sho 
startup. cc 
startup. old 


Directory  of  .  00:32:22 

AUVRUN  AUV  OLD 

CMDS  DEFS 

LIB  MACROS 

OS9boot  Read  Me 


SYSSRC 
evO .txt 
lp.d 

startlan 
startup. env 
startup. ori 


TEMP 
hello. c 
macph 

startlan. ori 
startup. full 
t2. 19200 


BYRNES 

ETC 

MISC 


WL 

junk 

mkrom.bat 
startup 
startup.ng 
t2 . 9600 


Figure  A.4  Script  of  laboratory  GESPAC  execution  of  NPS  AUV  control 
loop  software  (part  1) 
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$  *  comments  start  with  an  asterisk 

$  *  loop.c  is  the  closed-loop  control,  software  used  in  the  AUV' 
$  *  lp.d  is  the  run  geometry  data  fiie 
$  cc  loop.c  -k2f 
^op.c' 


$  *  Now  execute  loop.  Initial  values  were  values  read  from  lp.d  data  file. 
$  *  start_dwell  is  the  only  value  that  matters  right  now  since  it  is  the 
*  initial  delay  time. 

$  loop 

10.000000  0.000000  2.000000 
30.000000  0.000000  2.000000 
55.000000  3.140000  2.000000 
80.000000  3.140000  2.000000 
100.000000  6.280000  2.000000 
Input  start_dwell 

Input  k_psi  and  k_r 

5 

Input  k  z,  k  theta,  and  k  q 
1.1 
.5 
.5 

Input  k  speed  and  ki  speed 
4.0 
.5 

Input  speed  limit  from  1.0  to  3.0  feet/sec 
.5 

Input  rpm  from  +-200.0  to  +-650.0,  type  400.0 
1400.0 

Position  AUV  for  Directional  Gyro  Offset  Measurement 
Rate  Gyro  zero  measurement 
Hit  Any  Key  When  Ready 

pitch  0=0 
roll_0  =  0 
roll  rate_0  =  0 
pitcK_rate_0  =  0 
yaw_rate_0  =  0 
z_val0  =  0 

dg_offset  =  0.097793 
Starting 

Error  #000:002  keyboard  quit 

$  *  program  exit  was  accomplished  using  control-E  after  about  20  seconds 


Figure  A.5  Script  of  laboratory  GESPAC  execution  of  NPS  AUV  control 
loop  software  (part  2) 
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$  *  AUV  output  file  is  d.d  and  contains 
$  *  the  usual  floating  point  state  vector  at  10-  Hz. 

$  *  First-  parameter  is  clock  time. 

$  list  d.d 

0.000000  O.OC^OOO  0.000000  o.oboooo  0.000000  0'i 000000  -0.000000  o.oooooo 
0.000000  O.OOOOOO  -0.000000.  2.200000  0.000000  0.000000  o.oboooo  576.450000 

-8.479200  -8.479200.  .  "  '  '  ; 

0.100000  0.000000  0.000000  0.000000  0.000000:  0.000000  -0.000000  0.000000 
O.OOOOOO  0.000000  -0.000000  2.200000  0.000000  O.OOOOOO  0.000000  603.900000 
-8.479200-8.479200 

0.200000  Oi'OOOOOO  0.000000  0.000000  0.000000  0.000000  -0.000000  O.OOOOOO 
0.000000  OiOOOOOO  -0i000000  2.200000-  0.000000  ;0. 000000  0.000000-  631.350000 
-8.479200  -8.479200  , 

0.300000  o.oooooo  o.booooo  O.OOOOOO  O.OOOOOO  0.000000  -0.000000  0.000000 
0.000000  0.000000  -0,000000  2.200000  0.000000,0.000000  0.000000  -650.000000 
-8.479200  -8.479200 

0.400000  0.000000  0.000000: O.OOOOOO  0.000000  O.OOOOOO  -0.000000  0.000000 
0.000000  0.000000  -0.000000  2 ; 200000  0.000000  0.000000  0.000000  650.000000 
-8.479200  -r8. 479200 

0.500000  0.000000  0.000000  O.OOOOOO  0.000000  0,000000  -0.000000  0.000000 
0.000000  0.000000-0.000000  2.200000  0.000000  0..000000  0.000000  650.000000 
-8 i 479200  -8.479200 

0.600000  O.OOOOOO  0.000000  0.000000  0.000000  0.000000  -0.000000  0.000000 
0.000000  0.000000-0.000000  2.200000  0.000000  0.000000  0.000000  650.000000 
-8.479200  -8.479200 

0.700000  0.000000  O.OOOOOO  O.OOOOOO- O.OOOOOO  0.000000  -0.000000  0.000000 
0.000000  0.000000-0.000000  2.200000  0.000000  0.000000  0.000000  650.000000 
-8.479200  -8.479200 

0.800000  0.000000  0;000000  0.000000  0.000000  0.000000  -0.000000  0.000000 
0.000000  O.OOOOOO  -0,000000  2.200000  0.000000  0.000000  0.000000  650.000000 
-8.479200  -8.479200 

0.900000  0.000000  0.000000  0.000000  0.000000  0.000000  -0.000000  0.000000 
0.000000  0.000000  -o.oboooo  2.200000  0.000000  0.000000  0,. 000000  650.000000 
-8.479200  -8.479200 

1.000000  0.000000  0.000000  0.000000  0.000000  0.000000  -0.000000  0.000000 
0.000000  0.000000  -0.000000  2.200000  0.000000  0.000000  0.000000  650.000000 
-8.479200  -8.479200 

1.100000  O.OOOOOO  0.000000  0.000000  0.000000  0.000000  -0.000000  0.000000 
0.000000  0.000000  -0.000000  2.200000  0.000000  0.000000  OiOOOOOO  650.000000 
-8.479200  -8.479200 

1.200000  0.000000  0.000000  0.000000  0.000000  0.000000  -0.000000  0.000000 
0.000000  0.000000  -0.000000  2.200000  0.000000  0.000000  0.000000  650.000000 
-8.479200  -8.479200 

1.300000  0.000000  0.000000  0.000000  0.000000  0.000000  -0.000000  0.000000 
0.000000  0.000000  -0.000000  2.200000  0.000000  0.000000  0.000000  650.000000 
-8.479200  -8.479200 

1.400000  0.000000  0.000000  0.000000  0.000000  0.000000  -0.000000  0.000000 
0.000000  0.000000  -0.000000  2.200000  0.000000  0.000000  0.000000  650.000000 
-8.479200  -8.479200 

1.500000  0.000000  0.000000  0.000000  0.00 
$  Read  I/O  error  -  Error  #000:002  keyboard  quit 

$  *  Control-E  again  used  to  break  out 


Figure  A.6  Script  of  laboratory  GESPAC  execution  of  NPS  AUV  control 
loop  software  (part  3) 


* 
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1$  *  Now  let's  send  d.d  to  the  iris  for  use  by  the  integrated  simulator 


Form:  non-print 
■Prompting:  on 


Use  of  PORT  commands:  on 


Structure:  file 
Globbing:  on 


$  ftp  iirisl 

Connected  to  gravyl.cs.nps.navy.mil. 

220  gravy!  FTP  server  (IRIX  version  5.46  Aug  6  1990  10:17)  ready. 

Name  (gravyl. cs. nps. navy. mii:brutzman) : 

Password  (gravyl.cs. rips. navy. milfbrutzman) : 

331  Password  required  for  brutzman. 

230  User  brutzman  logged- in. 

Connected  to  gravyl; cs.nps.navy.mil. 

Mode:  stream  Type:  ascii  Form:  non-print  Structure:  file 

Verbose:  on  Bell:  off  '  Prompting:  on  Globbing:  on 

Hash  mark  printing:  off  Use  of  PORT  commands:  bn 

ftp>  put  d.d  labtest.d 
200  PORT  command  successful; 

150  Opening  ASCII  mode  data  connection  for  'labtest.d'. 

226  Transfer  complete. 

2575  bytes  sent  in  0.10  seconds  (25.15  Kbytes/s) 
ftp>  quit 

221  Goodbye. 

$  telnet  irisl 

Trying  131. 120. 1 .20 .. .Connected  to  gravyl.cs.nps.navy.mil. 

Escape  character  is 
capture  closed. 

IRIX  System  V.3  (gravyl) 

login:  brutzman 
Password: 

IRIX  System  V  Release  3.3.1  gravyl 

Copyright  (c)  1988,1989,1990  Silicon  Graphics,  Inc. 

All  Rights  Reserved. 

************************************ 

SCHEDULED  DOWN  TIME 
Backups  --  Wednesdays  0800-0900 

************************************ 


gravyl : /n/gravyl/work/brutzman 
%  Is 

AUV  backup. pool  graphics 

GA  bay_20m_data  lab.d 

Workspace  clips  labtest.d 


auto_pilot.c  d.d 


auv 

auvIII 


dumpster 

ff 


|auv_long_term  g 


laser 

laser2 

laser2h 

laserh 


nps 

off 

pline 

pline.c 

pool  m35.auv 

preview 

robotics 


tape.c 

temp 

titler 


gravyl : /n/gravyl/work/brutzman 
%  logout 

Connection  closed  by  foreign  host. 


Figure  A.7  Script  of  laboratory  GESPAC  execution  of  NPS  AUV  control 
loop  software  (part  4) 
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$  *  Note  file  d.-d  was  successfully  transferred  as  file  labtest.d  to  the 
iris 

$  *  also  note  ftp  and  telnet  can  be  used  back,  and  forth  as  well  as  nested. 
$  *  Lack  of  analog/digital  cards  did  not  prevent  loop.c  from  continuing. 

$  *  This  script  file  is  saved  as  auvsim.log 


S  *  Due  to  a  shell  glitch,  two  'logout'  commands  are  needed  to  exit  OS-9. 
$  logout 

brutzman>  lpgout 

Connection  closed  by  foreign  host. 
gemini:/n/gemini7work/brutzman» 


Figure  A.8  Script  of  laboratory  GESPAC  execution  of  NPS  AUV  control 
loop  software  (part  5) 
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APPENDIX  B.  NPS  AUV  GRAPHICS  SIMULATION  PROGRAM  SYNOPSIS 

The  NPS  AUV  Integrated  Simulator  graphic  simulation  program  is  written.in 
ANSI  C  to  run  on  Silicon  Graphics  Inc.  Ms  workstations.  Numerous  calls  are  made 
to  GL  Graphics  Library  routines.  Platform  dependence  and  length  (5200  lines  of 
code)  makes  complete  reproduction  of  the  source  program  and  support  files 
impractical.  Source  programs  are  summarized  in  this  appendix  and  are  available  via 

t 

Internet  as  described  in  Appendix  F. 

1.  GRAPHICS  SIMULATION  PROGRAM  STRUCTURE 

The  graphics  simulation  program  has  a  structure  typical  Of  most  Iris  applications. 
Graphics  functions  are  initialized,  data  is  initialized  and  a  world  view  is  drawn  from 
the'default  viewpoint  A  main  graphics  loop  is  then  repeated  indefinitely  which  reads 
user  inputs  (if  any),  redraws  the  world  view  and  outputs  updated  values.  Screen 
update  rate  is  dependent  on  workstation  capabilities  and  the  changing  complexity  of 
the  world  view  being  drawn.  Figure  B.l  shows  the  basic  simulation  graphics  loop  and 
summarizes  ail  major  functions. 

2.  NPS  PANEL  DESIGNER 

The  NPS  Panel  Designer  (NPSPD)  is  used  to  provide  a  user-friendly  control 
panel  interface  (King  Prevatt  90).  NPSPD  reads  high-level  text  files  that  specify 
layout  and  functionality  of  a  wide  variety  of  interface  icons  such  as  meters,  dials, 
knobs,  buttons  etc.  NPSPD  then  automatically  generates  "C"  source  code  functions 
that  can  be  tied  to  the  target  application  through  Makefile  entries  and  define 
statements.  Although  initial  setup  is  difficult,  use  of  NPSPD  permits  extensive 
changes  to  the  user  interface.  Subsequent  modifications  using  NPSPD  are  rapid  and 
convenient,  permitting  flexible  development  of  the  user  interface  during  simulator 
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Initialize  all  values  and  control  panel 


Transfer  values  from  control  panel 
Transfer  values  from  mouse/dials/buttons 
Process  commands:  read  file,  replay, 
change  viewpoint  or  meter  values, 
screen  snapshot,  reset,  exit  etc. 

Draw  background  environment  (pool) 

If  replay  in  progress: 
read  AUV  posture  from  replay  file 
pause/skip  record  if  in  real-time  mode 


Loop  for  all  objects,  including  AUV 
rotate/translate/scale  object 
draw  each  individual  object 


Screen  buffer  swap  (latest  in  foreground, 
background  ready  for  redraw) 

Transfer  updated  values  to  control  panel 
Redraw  control  panel 


Figure  B.l 


NPS  AUV  graphics  simulation  program 


development.  NPSPD  specifications  and  examples  are  found  in  (King  Prevatt  90)  and 
(Jurewicz.90). 

3.  GRAPHIC  OBJECT  MODELING  USING  OBJECT  FILE  FORMAT  (OFF) 
Creation  and  modeling  of  complex  graphic  objects  such  as  underwater  mines  of 
vehicles  can  be  extremely  tedious  if  only  using  GL  Graphics  Library  geometric 
primitive  function  calls.  Object  File  Format  (OFF)  has  been  developed  at  NPS  to 
encapsulate  most  of  the  functionality  of  GL  Graphics  Library  lighting,  drawing  and 
texturing  function  calls  in  a  manner  that  supports  modeling  and  manipulation  of 
high-level  graphics  objects  (Zyda  91).  Use  of  OFF  objects  greatly  simplifies  treatment 
of  individual  objects  modeled  in  a  simulated  world. 
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APPENDIX  C.  NPS  AUY  SONAR  CLASSIFICATION  SYSTEM  SOURCE 
CODE 


AUV  Sonar  Expert  System 
Filename:  auvsonar 

Purpose:  Batch  file  for  auvsonar. clp  which  resets  and  executes  the 

AUV  sonar  contact  classification  expert  system. 


Paper:  "Autonomous  Underwater  Vehicle  Sonar  Classification  using 

Expert  Systems  and  Neural  Networks" 

IEEE  OCEANS  '92  Conference,  Newport,  Rhode  Island 

Authors:  Don  Brutzman,  Mark  Compton  and  Dr.  Yutaka  Kanayama 


Date:  24  November  91 

Execution:  unix>  clipsS  unix>  clips5 

CLIPS>  (load  auvsonar. clp)  CLIPS>  (batch  auvsonar) 

CLIPS>  (reset)  CLIPS>  (run) 

CLIPS>  (run) 


;  Clear  C  close  files  in  case  they  were  left  open  during  previous  execution 

(clear)  ;  clear  all  facts  and  rules 

(close  rangefile)  ;  Close  AUV-recorded  pool  test  data  input  file 
(close  plotfile)  ;  Close  xy  coordinate  file  used  for  graph  output 

(close  auvfile)  ;  Close  expert  system  classification  output  file 


(load  auvsonar. clp) 

(undefrule  oldareal) 
(undefrule  oldarea2) 

(reset) 

(run) 


;  Load  in  AUV  Sonar  Classification  Expert  System 

;  Initialize  agenda  and  assert  initial  facts 
;  Execute  AUV  Sonar  Classification  Expert  System 
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AUV  Sonar  Expert  System 


Filename:  auvsonar.clp 


Purpose: 


Authors: 

Advisor: 


Define  data  templates,  rules,  functions  and' user  interface 
for  the  AUV  sonar  contact  classification  expert  system. 

"Autonomous  Sonar  Classification  using  Expert  Systems" 

IEEE  Oceanic  Engineering  Society 

IEEE  OCEANS  '92  Conference,  Newport,  Rhode  Island 

Don  Brutzman  and  Mark  Compton 
Dr.  Yutaka  Kanayama 

1  March  91 


Comments:  This  expert  system  takes  data  files  generated  by  the  NPS  AUV, 

uses  sonar  returns  and  AUV  position  to  generate  locations  of 
sonar  contacts,  perform  two-dimensional  linear  regression  to 

build  line  segments,  combine  segments  into  polyhedrons  and 
then  determines  the  probable  classification  of  each  polyhedron. 

Language:  CLIPS  MC"  Language  Integrated  Production  System 


Execution: 


References: 


History: 


Status: 


unix>  clipsS 
CLIPS>  (load  auvsonar.clp) 
CLIPS>  (reset) 

CLIPS>  (run) 


unix>  clipsS 
CLIPS>  (batch  auvsonar) 
CLIPS>  (run) 


Execution  'dribble'  files  are  saved  in  auvsonar.log 

_Sonar  Data  Interpretation  for  Autonomous  Mobile  Robots_, 

Yutaka  Kanayama,  Tetsuo  Noguchi,  and  Bruce  Hartman, 
unpublished  paper. 

Original  program  development  for  CS4311  Expert  Systems 
taught  by  Dr.  Kanayama. 

The  NPS  pool  coordinate  system  is  the  world  reference  used 
where  x  is  pool  length,  y  is  pool  width,  and  ».  is  pool  depth. 

Initial  development  complete  for  object  classification. 

Full  pool  depth  U3ed  for  pool  object  outputs. 

Initial  offset  option  for  centering  pool  data  included. 

Verbose  output  option  and  excess  data  retraction  completed. 

Gyro  error/gyro  drift  rate  evaluation  £  correction  implemented. 
Centroid  and  cross-sectional  area  calculations  done  for  objects. 
Top-level  classification  of  objects  using  area  is  possible. 

Mine  classification  implemented  satisfactorily. 

Excessively  narrow  objects  are  reclassified  as  walls. 


Data  Type  Deftemplates 


Data  template  and  slot  names  correspond  to  AUV  Data  Dictionary  definitions. 
Data  template  names  have  their  first  letter  capitalized. 

Variable  names  are  all  lower  case. 

CLIPS  data  types  and  symbols  used  in  symbolic  slots  are  capitalized. 
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) 


(type 

•NUMBER) 

(default 

0)  .  . 

(range 

0  ?VARIABLE)  )■ 

(field  z 

(type 

NUMBER) 

(default 

0)  , 

(range 

0  ? VARIABLE) ) 

(field  phi 

(type 

(default 

NUMBER) 

0)) 

(field  theta 

(type 

NUMBER) 

(default 

0)) 

(field  psi 

(type 

NUMBER) 

(default 

0)) 

(field  p 

(type 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  q 

(type 

NUMBER) 

(default 

0) 

(range 

0  ? VARIABLE) ) 

(field  r 

(type 

NUMBER) 

(default 

0) 

(range 

0  ? VARIABLE) ) 

(field  delta  dive  planes 
Ttype 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  delta  rudders 

(type 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  range_a 

(type 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  range_b 

(type 

NUMBER) 

(default 

0) 

(range 

0  ? VARIABLE) ) 

(field  range_c 

(type 

NUMBER) 

(default 

0) 

(range 

0  ? VARIABLE) ) 

(field  range_d 

(type 

NUMBER) 

(default 

0) 

(range 

0  ? VARIABLE) ) 

(field  valid_a 

(type 

INTEGER) 

(default 

1>) 

(field  valid_b 

(type 

INTEGER) 

(default 

in 

(field  valid_c 

(type 

INTEGER) 

(default 

in 

(field  valid_d 

(type 

INTEGER) 

(default 

1)) 

(field  speed 

(type 

NUMBER) 

(default 

0.0)) 

(field  processed 

(type 

SYMBOL) 

(default 

FALSE) 

(allowed-values  TRUE  FALSE) ) 


dead  reckoning  estimate  of  travel 
relative. £0  start  .position; 

element  of  Point_3D  AUV  data  type 
source:  pressure-sensing  depth  cell 
which  may  be  inaccurate1  when  shallow 

element  of  Attitude\3D  AUV-  data  type 
in  radians 
'(roll) 

element  of  Attitude_3D  AUV  data  type 
in  radians” 

(pitch) 

element  of  Attitude_3D  AUV  data  type 
in  radians.  Note~caveat  on  pg.  1 
(yaw) 

element  of  Point_3D  AUV"  data  type 
in  radians/sec- 


;  element  of  Point_3D  AUV  data  type 
;  in  radians/sec 


;  element  of  Point_3D  AUV  data  type 
;  in  radians/sec- 


;  change  in  bow/stern  planes  position 
;  in  degrees 


;  change  in  rudder  planes  position 
;  in  degrees 


!  0-4095  range  units  correspond  to 
;  0..30m  pool  or  0..300m  ocean. 


;  Up  to  4  transducers  can  be  included. 


;  Validity  signal  from  sonar  hardware 


;  AUV  speed  from  flow  sensor 


;  set  TRUE  when  point  is  asserted, 
;  FALSE  until  then. 


(deftemplate  Object_data 

(field  detection_time  ;  time  is  positive,  set  by  AUV 

(type  NUMBER) 

(default  0) 

(range  0  ?VARIABLE) ) 

(field  latest  time 


152 


(type 

NUMBER) 

(default 

0) 

(range 

0  ? VARIABLE) ) 

(field  valid 

(type 

INTEGER) 

(default 

0)) 

(field  x 

f 

object  center 

(type 

NUMBER) 

(default 

0) 

(range 

0  ’VARIABLE)) 

(field  y 

r 

object  center 

(type 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  z 

t 

object  center 

(type 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  accuracy 

(type 

FLOAT) 

(default 

0.0) 

(range 

0.0  7VARIABLE) ) 

(field  object 

(type 

INTEGER) 

(default 

0) 

(range 

0  9)) 

(field  length 

(type 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  height 

(type 

NUMBER) 

(default 

0) 

(range 

0  ? VARIABLE) ) 

(field  width 

(type 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  confidence 

• 

» 

normalized 

(type 

FLOAT) 

(default 

0.0) 

(range 

0.0  1.0) ) 

(deftomplate  Point 

(field  time  ;  time  is  positive,  set  by  AUV 

(type  NUMBER) 

(default  0) 

(range  0  7VARIABLE) ) 

(field  x  ;  element  of  Point  3D  AUV  data  type 

(type  NUMBER) 

(default  0) 

(range  0  7VARIABLE) ) 

(field  y  ;  element  of  Point_3D  AUV  data  type 

(type  NUMBER) 

(default  0) 

(range  0  7VARIABLE) ) 

(field  z  ;  element  of  Point  3D  AUV  data  type 

(type  NUMBER) 

(default  0) 

(range  0  7VARIABLE) ) 

(field  valid 

(type  INTEGER) 

(default  0)) 

(field  status 

(type  SYMBOL) 

(default  NEW) 

(allowed-values  NEW  ACTIVE  INVALID  ENDPOINT  USED)) 

) 


(deftemplate  Regression_line 

(field  start  ;  matches  time  of  start  point 

(type  NUMBER) 

(default  0) 

(range  0  7VARIABLE) ) 

(field  end  ;  matches  time  of  end  point 
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) 


(type  NUMBER) 

(default  0) 

(range  0  7VARIABLE) ): 

(field  r 

(type  FLOAT) 

(default  0.0) 

(range  0.0  7VARIABLE) ) 

(field  orientation  ;  normalized  degrees 

(type  FLOAT) 

(default  0.0) 

(range  0.0  7VARIABLE) ) 

(field  correlation 

(type  FLOAT) 

(default  0.0) 

(range  0.0  7VARIABLE) ) 

(field  status 

(type  SYMBOL) 

(default  NEW) 

(allowed-values  NEW  CURRENT  VALID  USED  USED  FOR  AREA) ) 


(deftemplate  Node 

(field  time 

(type 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  x 

(type 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  y 

(type 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  z 

(type 

NUMBER) 

(default 

0) 

(field  accuracy 

(range 

0  ’VARIABLE)) 

(type 

(default 

(range 

FLOAT) 

0.0) 

0.0  7VARIABLE) ) 

(field  confidence 

(type 

FLOAT) 

(default 

0.0) 

) 

• 

(range 

0.0  1.0)) 

(deftemplate  Edge 

(field  start 

;  slot  values  are  times  corresponding  to  data 

(type 

FLOAT) ) 

(field  end 

(type 

FLOAT) ) 

(field  averager 

(type 

NUMBER) 

(default 

0) 

(range 

0  7 VARIABLE) ) 

(field  status 

(type 

SYMBOL) 

(default 

USED) 

(allowed-values 

) 

t 

USED  USED_FOR_AREA) J 

(deftemplate  Curve 

;  not  yet  implemented 

(field  time 

(type 

NUMBER) 

(default 

0) 

(range 

0  7VARIABLE) ) 

(field  node 

,*  slot  values  are  times  corresponding  to  data 

(type 

FLOAT) ) 

(field  edge 

FLOAT) ) 

(type 

(field  shape 
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) 


(type  SYMBOL) ) 


(deftemplate  Polyhedron 

(field  start  ;'time  of  the  initial  node/edge/curve  element 

(type  NUMBER) 

(default  0) 

(range  0  ? VARIABLE) ) 

(field  end  ;  time  of  most  recent  node/edge/curve  element 

(type  NUMBER) 

(default  0) 

(range  0  ’VARIABLE) ) 

(field  startx 

(type  NUMBER) 

(default  0) 

(range  0  ?VARIABLE) ) 

(field  starty 

(type  NUMBER) 

(default  0) 

(range  0  ?VARIABLE) ) 

(field  startz 

(type  NUMBER) 

(default  0) 

(range  0  ?VARIABLE) ) 

(field  centroidx 

(type  NUMBER) 

(default  0) 

(range  0  ? VARIABLE) ) 

(field  centroidy 

(type  NUMBER) 

(default  0) 

(range  0  ’VARIABLE) ) 

(field  centroidz 

(type  NUMBER) 

(default  0) 

(range  0  ? VAR I ABLE) ) 

(field  sidecount 

(type  INTEGER) 

(default  1) 

(range  1  ?VARIABLE) ) 

(field  sidecounterl 


(type  INTEGER) 
(default  -1) 

(range  -1  ? VARIABLE)) 


(field  sidecounter2 

(type 

INTEGER) 

(default 

-1) 

(range 

-1  ^VARIABLE}) 

(field  area 

(type 

FLOAT) 

(default 

0.0) 

(range 

0.0  7VARIABLE) ) 

(field  height 

(type 

FLOAT) 

(default 

0.0) 

(range 

0.0  7VARIABLE) ) 

(field  accuracy 

(type 

FLOAT) 

(default 

0.0) 

(range 

0.0  7VARIABLE) ) 

(field  confidence 

(type 

FLOAT) 

(default 

0.0) 

(range 

0.0  1.0)) 

(field  trait 

(type 

SYMBOL) ) 

(field  status 

(type 

SYMBOL) 

(default 

ACTIVE) 

(allowed-values  ACTIVE  COMPLETE  USED  FOR_AREA) ) 

(field  classification 

(type  SYMBOL) 

(default  WALL) 

(allowed-values  NEW  CURRENT  WALL  OBJECT  MINE  SWIMMER  UNKNOWN 
SEA-MOUNT  SUBMARINE  SHIP_I_E_SKIMMER_PUKE 
BIOLOGICS  LOTS  OF  BIOLOGICS)) 
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'  Initialization  of  Flag  Facts 


(deffacts  initial-flags 

(start-new-window-flag) 
(retract-excess-data  TRUE) 
(location  pool) 

;;;;;;;  (location  ocean) 


;  commence  parametric  regression  process 
;  any  other  value  saves  excess  data 
;  NPS  swimming  pool  test  environment 
;  alternative  environment 


) 


Global  Constants 


(defglobal  ?*minimum_points_in_edge*  ■  5)  ;  hard  coded  into  regression  defrules 


(defglobal 

(defglobal 

(defglobal 

(defglobal 


?*transducer_a*  -  1) ;  forward  transducer  corresponds  to  slot  range_a 
?*transducer~b*  -  2);  left  transducer  corresponds  to  slot  range_b 
?*transducer_c*  "  3);  right  transducer  corresponds  to  slot  range~c 
?*transducer_d*  “4);  depth  transducer  corresponds  to  slot  range~d 


(defglobal  ?*feet_per_sonar_unit*  -  0.02398)  ;  0-4095  range  units  correspond  to 

;  0..30m  pool  or  0..300m  ocean. 


(defglobal  ?*correlation_confidence  weight*  -  1) 
(defglobal  ?*validity_confidence_weTght*  -  1) 


(defglobal  ?*cl*  -  3.00)  ;  max  #  standard  deviations  a  point  can  be  out 
(defglobal  ?*c2*  -  2.00)  ;  max  offset  distance  (feet)  allowed  from  line 
(defglobal  ?*c3*  -  0.066)  ;  min  regression  ellipse  thinness  requirement 


;  define  default  line/wall  color  strings 
(defglobal  ?*colorl*  *  "Color  0  2S5 

(defglobal  ?*color2*  -  "Color  200  255 

(defglobal  ?*color3*  »  "Color  0  78 

(defglobal  7*mln_wall_length*  -  1.0)  ; 

9 

(defglobal  ?*max_edge_distance*  -  7.0)  ; 

9 

(defglobal  ?*max_edge_angle*  »  10.0)  ; 

9 

(defglobal  ?*wall_thinness_ratio*  »  0.1)  ; 


78  1.0  0  255  78");  detected  edge 

150  0.7  200  255  150");  Inferred  edge 

255  0.5  0  78  255");  hidden  edge 

min  allowable  individual  edge  length 
to  be  output  as  a  WALL 

max  allowable  distance  between  edges 
for  edge-joining/polyhedron  building 

max  allowable  angle  between  edges 
for  edge-joining/WALL  building 

used  to  reclassify  long  skinny  object 
as  WALL 


Global  Variables 


(defglobal 

?*n* 

s 

0.0) 

m  00 

(defglobal 

?*sumx* 

0.0) 

m  10 

(defglobal 

?*sumy* 

S 

0.0) 

m  01 

(defglobal 

?*sumxy* 

* 

0.0) 

• 

1 

m  11 

(de fglobal 

?*sumxx* 

a 

0.0) 

m  20 

(defglobal 

?*sumyy* 

a 

0.0) 

9 

m  02 

(defglobal 

?*meanx* 

s 

0.0) 

mu  x 

(defglobal 

?*meany* 

= 

0.0) 

• 

# 

mu  y 

(defglobal 

?*sigmaxx* 

a 

0.0) 

M  20 

(defglobal 

?*sigmaxy* 

a 

0.0) 

• 

M  11 

(defglobal 

?*sigmayy* 

= 

0.0) 

9 

M  02 

(defglobal 

?*phi* 

a 

0.0) 

• 

9 

regression  line  orientation 

(defglobal 

?*r* 

0.0) 

• 

9 

regression  line  distance  from  origin 

(defglobal 

?*M-ma jor* 

= 

0.0) 

* 

9 

Moment  around  ellipse  major  axis 

(defglobal 

?*M-minor* 

“ 

0.0) 

• 

9 

Moment  around  ellipse  minor  axis 

(defglobal 

?*d-ma jor* 

= 

0.0) 

t 

diameter  on  ellipse  major  axis 

(defglobal 

?*d-minor* 

= 

0.0) 

9 

diameter  on  ellipse  minor  axis 
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(defglobal  ??rho*  =0.0) 
(defglobal  ?*delta*  =0.0) 
(defglobal  ?*sigma*  =  0.0) 


ratio  of  major  to  minor  axis  diameters 
residual  of  a  point  ' 
standard  deviation 


(defglobal  ?*pro jection-x*  =  0.0) 
(defglobal  ?*projection-y*  =  0.0) 


x  projection- of  point  i  on  major  axis 
y  projection  of  point  i  on  major  axis 


(defglobal  ?*minx*  =  1.45) 

(defglobal  ?*maxx*  =  -15) 


for  plot/graph  boundaries 


(defglobal  ?*miriy*  =  -50) 

(defglobal  ?*maxy*  =  110) 


for  plot/graph  boundaries 


(defglobal  ?*minz* 
(defglobal  ?*maxz* 


=  0.0) 
=  0.0) 


•;  for  the  currently  active  edge  only 
;  for  the  currently  active  edge  only 


(defglobal  ?*defaultz*  =2.0) 


default  pool  depth  to  be  used  for  objects 
of  unspecified  or  indeterminate  depth 


(defglobal  ?*offsetx*  =  0.0) 
(defglobal  ?*offsety*  =  0.0) 
(defglobal  ?*offsetz*  =  0.0) 


displacement  added  to  (x,  y,  z)  positional 
data  to  account  for  distance  of  the  AUV 
from  the  origin  (i.e.  corner)  of  the 
NPS  pool  coordinate  system 


(defglobal  ?*time* 
(defglobal  ?*out* 


=  0.0) 

=  stdout) 


;  used  to  measure  execution  time 

;  verbose  output  default  to  stdout 
;  otherwise  ?*out*  is  reset  to  nil 


(defglobal  ?*gyroerror*  =  0.0) 
(defglobal  ?*newgyroerror*  =  .0.0) 
(defglobal  ?*gyroerrortime*  =0.0) 

(defglobal  ?*gyrodriftrate*  =  0.0) 


(defglobal  ?*newgyrodriftrate*  =0.0) 
(defglobal  ?*number_of_fields*  »  17) 


;  User-provided  gyro  error  (degrees) 

;  Expert  system  gyro  error  (degrees) 

;  Average  time  of  first  wall  found,  used 
;  as  input  to  drift  rate  computation 

;  User-provided  drift  rate 
;  Expert  system  drift  rate 

;  read  only  actual  #  of  input  fields 


;  A  sample  fact  (for  syntax  training  use  only!) 

(deffacts  rangel  (Range  data  (time  0) 

(x  2)  (y  3)  (z  4) 

(phi  5)  (theta  6)  (psi  7) 

(p  8)  (q  9)  (r  10) 

(delta_dlve_plane3  11) 

(delta-rudders  12) 

(range~a  13)  (valid_a  1) 

(range~b  15)  (valid_b  1) 

(speed- 17)  (processed  TRUE) ) 

) 


t 

;  Functions 


;  atan2  function  matches  C  language  and  class  text  syntax. 
;  Calling  order:  (atan2  y  x) 


(defmethod  atan2  < (?y  NUMBER) (?x  NUMBER  (>  ?x  0) ) ) 
(atan  (/  ?y  ?x) ) ) 


(defmethod  atan2 
(+  (atan  (/  ?y 

( (?y 
?x) ) 

NUMBER 

(pi))) 

(> 

O 

>i 

(?x  NUMBER 

(< 

?x 

0))) 

(defmethod  atan2 
(-  (atan  (/  ?y 

( (?y 
?x)) 

NUMBER 

(pi))) 

(< 

?y  0)) 

(7x  NUMBER 

(< 

?x 

0))) 

(defmethod  atan2 
(/  (pi)  2.0)) 

((?y 

NUMBER 

(> 

?y  0)) 

(?X  NUMBER 

(  = 

?X 

0))) 

(defmethod  atan2 
(/  (pi)  -2.0)) 

( <?y 

NUMBER 

K 

?y  0)) 

(?X  NUMBER 

(= 

?x 

0))) 

(defmethod  atan2 

( (?y 

NUMBER 

(= 

?y  0)) 

(?X  NUMBER 

(< 

?x 

0))) 

(pi)) 
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(defmethod  atan2  ((’y  NUMBER  (=  ’y  0) )  {?x  NUMBER  (=  ?x  0) ) ) 

0.0) 


(deffunction  normalize  (?x)  ;  x  in  degrees,  resulting  range  (0  ..  360) 

(bind  ?norm  ?x) 

(while  (<  ?norm  0.0)  (bind  ?norm  (+  ?norm  360.0))) 

(while  (>=  ?norm  360.0)  (bind  ?norm  (-  ?norm  360;0))) 

?norm 


(deffunction  normalize2  (?x)  ;  x  in  degrees,  resulting  range  (-180  ..  180) 

(bind  ?norm  ?x) 

(while  (<  ?norm  -180.0)  (bind  ?norm  (+  ?norm  360.0))) 

(while  (>=  ?norm  180.0)  (bind  Jnorm  (-  ’norm  360.0))) 

?norm 


(deffunction  avg  (?numberl  ?number2) 
(/  (+  ?numberl  ?number2)  2.0)) 


(deffunction  degrees  (?x)  ;  x  in  radians 

(/  (*  ?x  180.0)  (pi)) 

) 


(deffunction  radians  (?x)  ;  x  in  radians 

(*  (/  ?x  180.0)  (pi)) 

) 


;  Boolean  function  to  ask  a  yes/no  question 

(deffunction  yes-or-no  (’question).  ;  '’question'  is  the  question  string 

(format  t  "%n%s?  "  ?question)  ;  ask  the  question 

(bind  ?answer  (lowcase  (sym-cat  (read)})) 

(while  (and  (neq  ?answer  yes)  (neq  ’answer  y)  (neq  ?answer  yep) 

(neq  ?answer  yeah)  (neq  ?answer  ye)  (neq  ?answer  yea) 

(neq  ?answer  no)  (neq  ?answer  n) 

(neq  ?answer  nope)  (neq  ?answer  nah) ) 

(format  t  "%n  Please  answer  yes  or  no:  ") 

(bind  ?answer  (lowcase  (sym-cat  (read))))) 

(if  (or  (eq  ?answer  yes)  (eq  ?answer  y)  (eq  ?answer  yep) 

(eq  ?answer  yeah)  (eq  ?answer  ye)  (eq  ?answer  yea) ) 
then  TRUE 
else  FALSE) 


(deffunction  distance  (?xl  ?yl  ?zl  ?x2  ?y2  ?z2) 

(sqrt  (+  (*  (-  ?xl  ?x2)  (-  ?xl  ?x2)) 

(*  (-  ?yl  ?y2)  (-  ?yl  ?y2) ) 

(*  (-  ?zl  ?z2)  (-  ?zl  ?z2) ) 

)) 

) 


Triangle  S  and  area  calculation  functions 


(deffunction  S  (?nodelx  ?nodely  ?node2x  ?node2y  ?node3x  ?node3y) 
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CCW  triples  are  positive  6  CW  triples  are.  negative,  matching-  conventions. 


(bind  ?trianglearea 

(*  0.5 


?trianglearea) 


(-  (*  (-  ?node2x  ?nodelx) 
(*  (-  ?node3x  ?nodelx) 


{-  ?node3y  ?nodely) ) 

(-  ?node2y  ?nodely) ) ) ) ) 


(deffunction  area  (?nodelx  ?nodely  ?node2x  ?node2y  ?node3x  ?node3y) 
;  area  values  are  always  positive,  matching  conventions. 


(bind  ?trianglearea 

(abs  {*  0.5  (-  (*  (-  ?node2x  ?nodelx) 
(*  {-  ?node3x  ?nodelx) 


?trianglearea) 


(-  ?node3y  ?nodely) ) 
?node2y  ?nodely) ) ) ) ) ) 


Expert  system  start  and  data  file  reading  rules 


(defrule  get-initial-expert-system-parameters-and-open-range-file 

(declare  (salience  100) ) 

(initial-fact) 

~> 

(dribble-off) 

(system  ”mv  -f  auvsonar.log  auvsonar.log. bak") 

(dribble-on  auvsonar.log) 

(printout  t  crlf  crlf  "Name  of  range  data  file  to  open?  ") 

(bind  ?filename  (read) ) 

(open  ?filename  rangefile  "r") 

(printout  t  "Opened  range  data  file  "  ?filename  crlf) 

(printout  t  crlf) 

(if  (yes-or-no  "Are  there  more  than  17  fields  per  Range_data  record") 
then  (printout  t  crlf  "Enter  number  of  data  fields  per  record:  ") 

(bind  ?*number_of_field3*  (read)) 

(while  (or  (<  ?*number  of_fields*  17)  (>  ?*number  of  fields*  20) ) 
(printout  t  crl7  "Enter  a  value  from  17..2TT:  ~") 

(bind  ?*number  of_fields*  (read) ) 

(printout  t  crTf  crlf))) 

;  Determine  output  device  for  trace  statements  using  'format  ?*out*' 
(printout  t  crlf). 

(if  (yes-or-no  "Do  you  want  verbose  output  onscreen  during  analysis") 
then  (bind  ?*out*  stdout) 
else  (bind  ?*out*  nil)) 

(printout  t  crlf) 

(if  (yes-or-no  "Do  you  want  to  input  gyro  error  and  gyro  drift  rate") 
then  (printout  t  crlf  "Enter  gyro  error  (degrees):  ") 

(bind  ?*gyroerror*  (normalize2  (read))) 

(printout  t  crlf  "Enter  gyro  drift  rate  (degs/hr) :  ") 

(bind  ?*gyrodrlftrate*  (read))) 

(printout  t  crlf) 

(printout  t  crlf) 

(printout  t  crlf  "Enter  offset  distance  to  be  added  to  X  positions  to  ") 
(printout  t  "account  for  the  initial  AUV  displacement  from  pool  corner:  ") 

(bind  ?*offsetx*  (read) ) 

(printout  t  crlf) 

(printout  t  crlf  "Enter  offset  distance  to  be  added  to  Y  positions  to  ") 
(printout  t  "account  for  the  initial  AUV  displacement  from  pool  corner:  ") 

(bind  ?*offsety*  (read)) 

(printout  t  crlf) 


(printout  t  crlf  "Enter  offset  depth  to  be  added  to  Z  positions  to  ") 
(printout  t  "account  for  the  initial  AUV  displacement  from  pool  surface:  ") 
(bind  ?*offsetz*  (read) ) 

(printout  t  crlf) 


(printout 

(printout 

(system 

(printout 

(printout 

(system 


crlf  "Saving  previous  files  pool. graph  and  pool.auv:" 
"mv  -f  pool.auv  pool.auv.bak") 

"mv  -f  pool.auv  pool.auv.bak") 
crlf) 

"mv  -f  pool. graph  pool. graph. bak") 

"mv  -f  pool. graph  pool .graph. bak") 


crlf) 
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(printout  t  crlf) 

(open  "pddl.auv"  auvfile  "a") 

(open  “pool -graph"  plotfile  "a") 

(printout  auvfile  crlf  crlf 

"  NPS  AUV  Sonar  Classification  Expert  System" 

-  "  . (pool  data  "  Pfilename  “) “ 

crlf  crlf -crlf) 

(printout  auvfile  crlf  “All  data  values  t;type  specifications  are  * 

,  "defined  -by  the  AUV  Data  Dictionary." 
crlf) 

(printout  auvfile  crlf  "All  coordinate  values  are  relative  to  the  " 

"NPS  Pool  Coordinate  System." 
crlf  crlf  crlf) 

(printout  auvfile  crlf  "AUV-  “ 

?*offsetx*  "  "  ?*offsety*  "'  "'  ?*of fsetz* 

"  (xyz  distances  from  AUV  start  position  to  pool  origin) "  crlf) 

(printout  plotfile  "105.0  95.0  "  crlf  "\"NPS  AUV  Sonar  Classification  " 

"Expert  System  (pool  data  "  Jfilename  ")  \"  “ 
crlf) 

(printout  plotfile  "  100.0  -30.0  "  crlf  "\"AUV  start. .origin  offset  values:  " 

?*offsetx*  ",  " 

?*offsety*  ",  " 

?*offsetz*  "  \"  " 

crlf) 

(printout  plotfile-  "  105.0  -40.0  "  crlf  "\"Parametric  regression  constants:  " 

"cl="  ?*cl* 

",  c2=7.  ?*c2* 

",  c3~"  ?*c3*  "  \"  " 

crlf) 

(printout  plotfile  "  0.0  0.0  "  crlf  ;  pool  boundary  outline 

"  127.0  0.0  "  crlf 

"  127 ;0  67.5  "  crlf 

"  0:0-  67.5  "  crlf 

"  0;0  0.0  "  crlf  "  \"  \"  " 

crlf) 

(printout  auvfile  crlf  "Environment  nps  pool. off" 

crlf  "Replayfile  "  ’filename 

crlf  "Replaysize  "  ?*number_of  fields* 

crlf)  ;  simulator  replay  filename/Tilesize  initialization 

(if  (or  (<>  ?*gyroerror*  0.0)  (<>  ?*gyrodriftrate*  0.0))  then 

(printout  plotfile  "  100.0  -20.0  "  crlf  "\"AUV  gyro  error  -  " 

?*gyroerror*  "  degrees,  gyro  drift  rate  =  " 

?*gyrodriftrate*  "  degrees/hour  \"  " 

crlf)) 

(if  (or  (<>  ?*gyroerror*  0.0)  (<>  ?*gyrodriftrate*  0.0))  then 

(printout  auvfile  "gyroerror  "  ?*gyroerror*  "  degrees"  crlf 

"gyrodriftrate  "  ?*gyrodriftrate*  "  degrees/hour" 
crlf) ) 

(printout  auvfile  crlf  ?*colorl*  "  Color  scheme  for  regression  lines  " 
crlf)  ;  primary  default  color  scheme 

(bind  ?*time*  (time) )  ;  start  clock  timer 

(assert  (check-file-flag)) 

) 


(defrule  check-range-file 

?check-file  <-  (check-file-flag) 

(not  (range-file-closed-flag)) 

H> 

(retract  ?check-file)  ;  don't  read  this  file  again  until  point  is  processed 

(assert  (first-element-read-fi’e-flag  =  (read  rangefile) ) ) 

;  first-element-read-file-flag  will  be  asserted  with  first  element  from 
;  the  rangefile 

) 
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(defrule  skip-rarigefile-cc=sents  ;  keep  reading  the  file  until  we  get  a  number 
(declare  (salience  100) ) 

’first-eleoent-read-file  <- (first-element-read-file-flag  ?file-elerer.t  i  -EOF) 
(test  (not  (nirr.be rp  ’file-elemer.t) )  ) 

=> 

(retract  ’first-eleiaent-read-file) 

(printout  t  , 

{readline  rangefile)  flush  ■  consents  through  end-of-line 

(assert  (check-file-flag) ) 


(defrule  read-renainder-of-range-record 


(declare  (salience  100) ) 

?first-element-read-file  <-  (first-element-read-file-flag  ?file-element  i  -EOF) 
(test  (numberp  ?file-eier.ent) ) 

=> 

(retract  ?first-elenent-read-file) 

(bind  ’fieldl  ’file-element) 

(bind  ?field2  (read  rangefile)) 

(bind  ?field3  (read  rangefile) ) 

(bind  ?field4  (read  rangefile) ) 

(bind  ?fieid5  (read  rangefile) ) 

(bind  ?field6  (read  rangefile) ) 

(bind  ?field7  (read  rangefile) ) 

(bind  ?field8  (read  rangefile) ) 

(bind  ?field9  (read  rangefile) ) 

(bind  JfieldlO  (read  rangefile) ) 

(bind  ?fieldll  (read  rangefile) ) 

(bind  7fieldl2  (read  rangefile) ) 

(bind  ?fieldl3  (read  rangefile) ) 

(bind  ?fieldl4  (read  rangefile}) 

(bind  ?fieldl5  (read  rangefile) ) 

(bind  ?fieldl6  (read  rangefile) ) 

(bind  ?fieldl7  (read  rangefile) ) 

(if  (>=  ?*number_of_fields*  18)  then  (bind  ?fieldl8  (read  rangefile) ) ) 

(if  (>»  ?*number_of  fields*  19)  then  (bind  ?fieldl9  (read  rangefile))) 

(if  (>-  ?*number_of^fields*  20)  then  (bind  ?field20  (read  rangefile))) 

;  account  for  user-provided  gyro  error  and  gyro  drift  rate: 

(bind  ’totalerror  (radians  (+  ?*gyroerror* 

(*  ?*gyrodriftrate*  (/  ’fieldl  3600.0))))) 

(bind  ?heading  (-  ?field7  ?totalerror) ) 

;  Don't  assert  a  point  if  it  has  no  range  value  (non-return) 

(if  (or  (>  ?fieldl3  1)  (>  ?fieldl4  1)  (>  ?fieldl5  1)  (>  ?fieldl6  1)) 
then 

(assert  (Range  data  (time  ’fieldl) 

(x  ?field2) 

(y  ?field3) 

(z  ?field4) 

(phi  ?field5) 

(theta  ?field6) 

(psi  ’heading) 

(p  ?field8) 

(q  ?field9) 

(r  ?fieldl0) 


(assert  (Range  data  (time  ’fieldl) 

(x  7field2) 

(y  ?field3) 

(z  ?field4) 

(phi  ?field5) 

(theta  ?field6) 

(psi  ’heading) 

(p  ?field8) 

(q  ?field9) 

(r  ’fieldlO) 

(delta_dive_planes  7fieldll) 

(delta~rudders  ?fieldl2) 

(range_a  ?fieldl3) 

(range_b  ?fieldl4) 

(range~c  ?fieldl5) 

(range_d  ?fieldl6) 

(speed  ?fieldl7)))) 

(format  ?*out*  "%nCompleted  reading  range  record;  data  time  %3.1f"  ?fieldl) 

(assert  (check-file-flag)) 


(defrule  close-range-file 
(declare  (salience  100) ) 

?first-element-read-file  <-  (first-element-read-file-flag  EOF) 

=*> 

(retract  ?first-element-read-file) 
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{close,  rangefile) 

(assert  (range-file-closed-flag) ) 

(assert  (Point  (status  NEW)))  ;  dummy  point  so  last  line  (if  any)  is  saved 
(printout  t  crlf  "Closed  the  input  range  file."  crlf) 


Point  position  calculation  functions 


Forward  transducer  (#1):  reference  frame  is  identical  to  AUV 

Left  transducer  (#2) s  psi  -  AUV  psi  +  PI  /  2 

Right  transducer  (#3):  psi  -  AUV  psi  -  PI  /  2 

Depth  transducer  (14):  theta  ■  AUV  theta  +  PI  /  2 


(deffunction  delta_x  (?range  iphi  Jtheta  ?psi) 


(if  {-  ?*transducer_b*  1) 

then  (bind  ?result  (*  ?range  (*  (cos  ?theta) 
(if  (-  ?‘transducer  b*  2) 

then  (bind  Tresult  (*  ?range  (*  (cos  ?phl) 
(it  (-  ?*transducer_b*  3) 

then  (bind  ?result  (*  ?range  {*  (cos  ?phi) 
(if  (-'  ?*transducer_b*  4) 

then  (bind  Tresult  (*  ?range  (sin  ?t'neta)))) 
Tresult) 


(cos  ?psi))))) 

(cos  (-  ?psi  (/  (pi)  2))))))) 
;  I  Note  caveat  about  yaw 
(cos  (+  ?psi  (/  (pi)  2))))))) 


(deffunction  delta_y  (?range  ?phi  ?theta  ?psi) 

(if  (»  ?*transducer_b*  11 

then  (bind  ?result  (*  ?range  (*  (co3  ?theta) 
(if  (-  ?*transducer_b*  2) 

then  (bind  ?result  (*  ?range  (*  (cos  ?phi) 
(if  (“  ?*tra;isducor_b*  3) 

then  (oind  ’result  {*  ?rango  (*  (cos  ?phi) 
(if  (■  ?*transducor_b*  4) 

then  (bind  ?result  (*  ?range  (sin  ?phi) ) ) ) 
’result) 


(sin  ?psi))))) 

(sin  (-  ?psi  (/  (pi)  2))))))) 
;  1  Note  caveat  about  yaw 

(sin  (+  ?psi  {/  (pi)  2))))))) 


(deffunction  delta_z  Orange  ?phl  ?theta  ?psl) 

(if  (-  ?*transducor_b*  1) 

then  (bind  ?result  (*  ?ranga  (sin  ?theta)))) 

(if  (“  ?*transducer  b*  2) 

then  (bind  ?rcsult  (-  0  (*  Jrange  (sin  ?phi) ) ) ) ) 

(If  {«  ?''trsn3ducer_b*  3) 

then  (bind  ?re.sult  (*  ?range  (sin  ?phi) ) ) ) 

(if  (»  ?*transducer_b*  4) 

then  (bind  fresult  (*  Vrz.nge  {*  (cos  .’phi)  (cos  ?theta)))>) 
?result) 


Point  building  rule 


(defrule  build-point-frcm-raw-AUV-range-data 
;  this  rule  currently  handles  only  left  transducer 
(declare  (salience  200)) 

?range_data<-  (Range_data  (processed  FALSE) 

(time  ?timo)  (x  ?x)  (y  ? y)  (z  ?z) 

(phi  ?phi)  (theta  ?theta)  (psi  ?psi) 

(range_b  ?range) (valid_b  ?valid) ) 

(test  (<>  ?*transducer_b*  0)) 

(bind  ?range  (*  ?range  ?*feet_per  sonar_unit*) ) ;  unit  conversion  of  range  slot 
(bind  ?delta_x  (delta_x  ?range  ?pHi  ?theta  ?psi)) 

(bind  Tdelta^F  (delta_y  ?range  ?phi  ?theta  ?psi)) 

(bind  ?delta_z  (delta_z  ?range  ?phi  ?theta  ?psi)) 

(it  (and  (>  ?time  0)  (>  ?range  1))  then  ;  only  make  valid  data  points 

(assert  (Point  (time  ?time) 

(x  =(+  ?x  ?delta_x) ) 

(y  =  (+  ?y  ?delta_yj) 

(z  =(+  ?z  ?delta  z)) 
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(valid  ?valid) 

(status  NEW) ) ) 

;  print  sonar  return  as  'o'  and  auv  position  as  '*' 

(printout  plotfile  (+  ?x  ?delta  x.  ?*offsetx*)  "  " 

(+  ?y  ?delta3'  ?*offsety*)  crlf  "o"  crlf) 

(printout  plotfile  (+  ?x  ?*offsetx*)  "  " 

(+  ?y  ,?*offsety*)  crlf  crlf) 

;  include  coordinate  offsets 
(modify  ?range_data  (processed  TRUE)  (range  b  ?range)) 

(format  ?*out*— "%nAsserted  and  plotted  a  poTnt  for  data  time  %3.1f"  ?time)) 
else  ;  a  bogus  point 

(modify  ?range  data  (processed  TRUE)  (range  b  ?range)) 

) 


Two-dimensional  parametric  regression  line  analysis  rules 


(defrule  regression-line-sliding-window-s tart-criteria 


(declare  (salience  300)) 

?start-new-window  <-  (start-new-window-flag) 

;  Find  the  next  S  NEW  points 
?pointl  <-  (Point  (status  NEW)  (time  ?timel) (x 

?point2  <-  (Point  (status  NEW)  (time  ?time2) (x 

?point3  <-  (Point  (status  NEW)  (time  7time3) (x 

7point4  <-  (Point  (status  NEW)  (time  7time4) (x 

?point5  <-  (Point  (status  NEW)  (time  7time5)  (x 

(test  (<  ?timel  7time2)) 

(test  (<  ?time2  7time3)) 

(test  (<  ?time3  7time4)) 

(test  (<  ?time4  7time5)) 


7x1) 

(y 

?yl) 

(z 

7x2) 

(y 

?y2) 

(z 

7x3) 

(y 

?y3) 

(z 

7x4) 

(y 

7y4) 

(z 

7x5) 

(y 

?yS) 

(z 

?zl)) 
7z2)> 
7z3) ) 
?z4) ) 
7z5)> 


(rotract  ?start-new-window) 

;  These  points  are  eligible  and  thus  become  ACTIVE 
(modify  ?pointl  (status  ACTIVE)) 

(modify  ?point2  (status  ACTIVE)) 

(modify  ?point3  (status  ACTIVE)) 

(modify  ?point4  (status  ACTIVE)) 

(modify  ?point5  (status  ACTIVE)) 

(bind  ?*n*  5) 

(bind  ?*sumx*  (+  ?xl  ?x2  7x3  7x4  7x5) ) 

(bind  ?*3umy*  (+  7yl  7y2  7y3  7y4  7y5)) 

(bind  7*sumxy*  (+  («  7x1  ?yl)  (*  7x2  7y2)  !*  7x3  7y3)  (*  7x4  7y4)  (*  7x5  7y5) ) ) 

(bind  7*sumxx*  (+  (*  7x1  7x1)  (»  7x2  7x2)  (*  7x3  7x3) (*  7x4  7x4)  (*  7x5  7x5))) 

(bind  ?*sumyy*  (+  (*  ?yl  ?yl)  (*  7y2  7y2)  (*  7y3  7y3) (*  7y4  7y4)  (*  7y5  7y5))) 

(bind  ?*minz*  (min  ?zl  7z2  7z3  7z4  ?z5) ) 

(bind  7*maxz*  (max  ?zl  7z2  7z3  7z4  7z5) ) 


(assert  (Regression_line  (start  ?timel)  (end  7time5)  (status  NEW))) 
(format  ?*out*  "InlnRegression  line  sliding  window  start  criteria  met.") 


(deffunction  calculate-line-fit-and-update-global-variables  0 
;  global  inputs:  n,  sumx,  sumy,  sumxy,  sumxx,  sumyy 


(bind  ?*meanx*  (/  ?*sumx*  ?*n*)) 
(bind  ?*meany*  (/  ?*sumy*  ?*n*)) 


(bind  ?*sigmaxx*  (-  ?*sumxx*  (/  (*  ?*sumx*  ?*sumx*)  ?*n*))) 
(bind  ?*sigmaxy*  (-  ?*sumxy*  {/  (*  7*sumx*  ?*sumy*)  ?*n*))) 
(bind  ?*sigmayy*  (-  ?*sumyy*  (/  (*  ?*sumy*  ?*sumy*)  ?*n*))) 


(bind  ?*phi*  (*  0.5  (atan2  (*  -2.0  ?*sigmaxy»)  (-  ?*sigmayy*  ?*sigmaxx*)) 
))  ;  note  paper's  caveat  re  frame  of  reference  of  phi 


(bind  ?*r*  (+  (*  ?*meanx*  (cos  ?*phi*))  (*  ?*meany*  (sin  ?’,phi*)))) 


(bind  7term2  (sqrt  (+  (*  0.25  (-  ?*sigmayy*  ?*sigmaxx*) 

(-  ?*sigmayy*  ?*sigmaxx*)) 
(*  ?*sigmaxy*  ?*sigmaxy*) ) ) ) 


(bind  ?*M-major*  (-  (/  (+  ?*sigmaxx*  ?*sigmayy*)  2.0)  7term2) ) 
(bind  ?*M-minor*  (+  (/  (+  ?*sigmaxx*  ?*sigmayy*)  2.0)  7term2)) 


(bind  ?*d-major*  (*  4  (sqrt  (/  ?*M-minor*  ?*n*)))) 
(bind  ?*d-minor*  (*  4  (sqrt  (/  ?*M-major*  ?*n*)))) 
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(bind  ?*rho*  (/  ?*d-mlnor*  ?*d-major*j) 

(format  ?*out*  ','%nRegression  line  fit  calculations  complete.") 


(defrule  regression-line-ihitial-segment-validity-check 
(declare  (salience  300) ) 

;  Get  the  NEW  Regression  line  and  5  ACTIVE- Points 

?line  <-  (RegressionJLlne  (start  ?timel)  (end  ?time5)  (status  NEW) ) 

?pointl  <-  (Point  (time  ?timel)  (x  Pxl)  (y  ?yl)  (z  ?zl)  (valid  ?validl)) 

?pointS  <-  (Point  (time  ?timeS)  (x  7x5)  (y  ?yS)  (z  Pz5)  (valid  PvalidS)) 

?point2  <-  (Point  (time  ?time2)  (x  ?x2)  (y  ?y2)  (z  ?z2)  (valid  ?valid2) ) 

(test  (and  (<  ?timel  ?time2)  (>  Ptime5  ?time2))) 

?point3  <-  (Point  (time  ?time3)  (x  ?x3)  (y  ?y3)  (z  ?z3)  (valid  ?valid3) ) 

(test  (and  (<  ?timel  ?time3)  (>  Ptime5  ?time3)  (<>  ?time2  ?time3))) 

Ppoint4  <-  (Point  (time  Ptime4)  (x  7x4)  (y  7y4)  (z  7z4)  (valid  7valid4)> 

(test  (and  (<  7timel  7time4)  (>  7time5  7time4)  (<>  7time2  7time4) 

(<>  7time3  7time4))) 

»> 

(calculate-line-fit-and-update-global-variables) 

(bind  ?*rho*  (/  ?*d-minor*  ?*d-major*)) 

(if  (<  ?*rho*  7*c3*)  ;  Validity  check:  Test  II  equation  (25) 

then  ;  initial  line  segment  IS  valid 

(modify  Ppointl  (status  ENDPOINT) ) 

(modify  7point2  (status  USED) ) 

(modify  7point3  (status  USED) ) 

(modify  ?point4  (status  USED)) 

(modify  7point5  (status  ENDPOINT)) 

(modify  ?line  (status  CURRENT) 

(r  ?*r*) 

(orientation  -(normalize  (degrees 

(atan2  (-  7y5  ?yl)  (-  7x5  7x1))))) 

(correlation  ?*rho*)) 

(format  ?*out*  "%nRegression  line  initial  segment  validity  check  passed.") 

else  ;  initial  line  segment  IS  NOT  valid 

(modify  ?pointl  (status  INVALID))  ;  window  slides  by  one  to  the  right 
(modify  7point2  (status  NEW)) 

(modify  7point3  (status  NEW)) 

(modify  7point4  (status  NEW)) 

(modify  7point5  (status  NEW)) 

(retract  ?line) 

(assert  (start-new-window-flag) )  ;  begin  building  a  new  window 

(format  ?*out*  "%nRegresslon  line  initial  segment  validity  check  failure.") 
(format  ?*out*  "%n") 

) 

) 


(defrule  regression-line-window-expansion 
(declare  (salience  300) ) 

;  Get  the  CURRENT  Regression_line,  start  Point,  end  Point,  and  new  Point 
?current-line  <-  (Regression-line  (start  Pstarttime)  (end  Pendtime) 

(status  CURRENT)) 

?new-point  <-  (Point  (time  Pnewtime)  (x  ?newx)  (y  ?newy) (z  ?newz) 

(status  NEW) ) 

?start-point  <-  (Point  (time  Pstarttime) (x  Pstartx) (y  Pstarty) (z  Pstartz)) 
Pend-point  <-  (Point  (time  Pendtime)  (x  Pendx)  (y  Pendy)  (z  Pendz) ) 

=  > 

(bind  ?*delta*  (+  (*  (cos  ?*phi*) {-  ?*meanx*  Pnewx)) 

(*  (sin  ?*phi*) (-  ?*meany*  Pnewy))))  ;  residual 

(bind  ?*sigma*  (sqrt  (/  ?*M-mino,r*  (-  ?*n*  2)))) 

(if  (and  (<  ?*delta*  (max  (*  ?*cl*  ?*sigma*)  P*c2*))  ;  Test  I  equation  (23) 

K  ?*rho*  P*c3*)  ;  Test  II  equation  (25) 

(>  Pnewtime  0) )  ;  ignore  invalid  points 


then  ;test  passed,  new  point  meets  criteria 

(modify  Pnew-point  (status  USED))  ;  we  just  used  this  point 
(bind  ?*n*  (+  ?*n*  D) 

(bind  ?*sumx*  (+  ?*sumx*  Pnewx) ) 
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(bind  ?*sumy*  (+  ?*sumy*  Jrtewy)  )• 

(bind  ?*sumxy*  (+  ?*sutnxy*  (*  ?newx  ?newy) ) ) 

(bind  ?*sumxx*  (+  ?*sumxx*  (*  ?newx  ?newx) ) ) 

(bind  ?*sumyy*  -(+  ?*sumyy*  (*  ?newy  ?newy) )  ) 

(bind, ?*minz*  (min  ?*minz*  ?newz) ) 

(bind  ?*maxz*  (max  ?*maxz*  ?newz) ) 

;update  globals  and  then  line  parameters 
(calculate-line-fit-and-update-global-variables) 

(bind  ?correlation  (-  1  ?*rho*)) 

;  update  endpoint  status  slots  for  possible  retraction  of  used  data 
(modify  ?end-polnt  (status  USED)) 

(modify  ?new-point  (status  ENDPOINT) ) 

(modify  ?current-line  (r  ?*r*) 

(orientation  -(normalize  (degrees 

(atan2  (-  Jnewy  ?starty)  (-  ?newx  ?startx) ) ) ) ) 
(correlation  Jcorrelation)  ;  value  range  [l-c3,.l] 
(end  Tnewtlme)) 

(format  ?*out*  "  Added  another  point  to  the  regression  line.»n"); 

else 

(modify  ?current-line  (status  VALID))  ;  test  failed,  save  old  line 

; current  point  retains  status  NEW  unless  it  is  a  dummy  point  at  time  zero 
(if  (»  0  ?newtlme)  then  (retract  ?new-point)) 

;  initial  node  of  new  segment 

(bind  ?*delta*  (+  (*  (cos  ?*phi») (-  ?*meanx*  ?startx)) 

(*  (sin  ?*phi*)  (-  ?*meany*  ?starty))))  ;  residual 

(bind  ?*pro jection-x*  (*  ?startx  {*  ?*dalta*  (cos  ?*phi*)))) 

(bind  ?*projection-y*  (+  ?starty  (*  ?*delta*  (sin  ?*phl*)))) 

(bind  ?start-pro jection-x  ?*projection-x*) 

(bind  ?start-projection-y  ?*projection-y*) 

(bind  ?correlation  (-  1  ?*rho*)) 

(assert  (Node  (time  ?starttime)  ;  edge's  virtual  start  node 

(x  ?*projection-x*) 

(y  T^projection-y*) 

(z  ?startz) 

(accuracy  ?*d-minor*)  ;  minor  axis  diameter 
(confidence  ?correlation) ) )  ;  using  elliptical  thinness 
(format  ?*out*  "  Valid  node  completed,  data  time  I3.1fln"  ?starttime) 

(printout  plotfile  (+  ?*pro jection-x*  ?*offsetx*)  "  " 

(+  ?*projection-y*  ?*offsety*)  crlf) 

(format  ?*out*  "  Projection  endpoints  (%S.lf,  %5.1f)M 

(+  ?*projection-x*  ?*offsetx*) 

(+  ?*pro jection-y*  ?*offsety*)) 

;  final  node  of  new  segment 

(bind  ?*delta*  (+  (*  (cos  ?*phi*)  (-  ?*meanx*  ?endx) ) 

(*  (sin  ?*phi*) (-  ?*meany*  ?endy))))  ;  residual 

(bind  ?*pro jection-x*  {+  ?endx  (*  ?*delta*  (cos  ?*phi*)))) 

(bind  ?*projection-y*  (+  ?endy  (*  ?*delta*  (sin  ?*phi*))i) 

(bind  ?confidence  (-  1  ?*rho*)) 

(assert  (Node  (time  ?endtime)  ;  edge's  virtual  end  node 

(x  ?*projection-x*) 

(y  ?*projection-y*) 

(z  ?endz) 

(accuracy  ?*d-minor*)  ;  minor  axis  diameter 

(confidence  ?confidence) ) )  ;  using  elliptical  thinness 

(format  ?*out*  "  (%5.1f,  %5.1f)%n" 

(+  ?*projection-x*  ?*offsetx*) 

(+  ?*projection-y*  ?*offsety*)) 

(format  ?*out*  11  Raw  data  endpoints  (%5.1f,  %S.lf)  (%5.1f,  %S.lf)%n" 

(+  ?startx  ?*offsetx*) 

(+  ?starty  ?*offsety*j 
(+  ?endx  ?*offsetx*) 

(+  ?endy  ?*offsety*)) 

(format  ?*out*  "  Valid  node  completed,  data  time  %3.1f  %n"  ?endtime) 
(printout  plotfile  (+  ?*projectlon-x*  ?*offsetx*)  “  " 

(+  ?*projection-y*  ?*offsety*)  crlf  M\"  \""  crlf) 

(assert  (Edge  (start  ?starttime) 

(end  ?endtime) 
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(averagez  y(avg  ?*minz*  ?*maxz*),) ) )' 

(format  ?*out.*  "  Valid  edge  completed,  data  times'  (%3;lf  ..  %3.1f)," 
Tstarttime  ?endtime)' 

(format  ?*out*  "  averagez  =  %3.1f,  line  r  =  %3.1f*" 

(avg  ?*minz*  ?*maxz*)  ?*r*) 

(format  ?*out*  "  line  orientation  »  %3.1f  degreesin" 

(normalize  (degrees  (atan2  (-  ?newy  ?starty)  (-  ?iiewx  ?startx))))) 

(format  auvfile 

"%nPoint  %5.1f  14. If  %3.1f  time  %4.1f" 

(+  ?start-projection-x  ?*offsetx*) 

(+  ?start-projection-y  ?*offsety*) 

(+  ?startz  ?*offsetz*) 

?starttime) 

;  depth  range  0..8  ft,  time  is  optional 

(format  auvfile 

"%nPoint  tS.lf  14. If  %3.1f  time  %4.1f" 

(+  ?*pro jection-x*  ?*off3etx*) 

(+  ?*pro jection-y*  ?*offsety*) 

(+  ?endz  ?*offsetz*), 

?endtime) 

(format  auvfile 

"%nSegment  %S.lf  »4.1f  %3.1f  »S.lf  »4.1f  %3.1f  time  %4.1f" 

(+  ?start-projection-x  ?*offsetx*) 

(+  ?start-pro jection-y  ?*offsety*) 

(+  (/  (+  ?*minz*  ?*maxz*)  2.0)  ?*offsetz*) 

(+  ?*projection-x*  ?*offsetx*) 

(+  ?*pro jection-y*  ?*offsety*) 

(+  (/  (+  ?*minz*  ?*maxz*)  2.0)  ?*offsetz*) 

?endtime) 

(assert  (check-file-flag)) 

(assert  (start-new-window-flag) ) 

(format  ?*out*  "  Valid  regression  line  actions  completed,  data  time  %3.1f" 
?endtime) 

(format  ?*out*  "%n4n") 

) 


Rules  for  retraction  of  excess  data  facts  (garbage  collection) 


(defrule  retract-excess-Range_data 

(retract-excess-data  TRUE) 

?range_data  <-  (Range_data  (processed  TRUE) ) 

«*> 

(retract  ?range_data) 

) 

(defrule  retract-excess-Point 

(retract-excess-data  TRUE) 

?point  <-  (Point  (status  INVALID  I  USED)) 

■> 

(retract  ?point) 

> 

(defrule  retract-excess-endPoint 
(retract-excess-data  TRUE) 

?point  <-  (Point  (status  ENDPOINT)  (time  ?point-time) ) 

?node  <-  (Node  (time  ?node-time) ) 

(test  (=  ?point-time  ?node-time) ) 

*> 

(retract  ?point) 

> 


Gyro  error  rules 


(defrule  determine-initial-gyro-error 
(declare  (salience  300) ) 

?pool  <-  (location  pool)  ;  this  rule  only  works  in  the  pool 
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?poly  <-  (Polyhedron  (classification  WALL)  (start  ?polystart)  (end  ?polyend) 
(status-  COMPLETE) ) 

(test  (»  ?*newgyroerror*  0.0))  ;  first  wall  provides  best  est,  don't  repeat 
?line  <-  (Regression_line  (start  ?start) (end  ?end) (orientation,  ^orientation) 
(status  USED  |  USED_FOR_AR£A) ) 

(test  («•  ?polyend  ?end) ) 

?pointl  <-  (Point  (time  ?timel)  (x  ?xl)  (y  ?yl)  (z  ?zl)) 

(test  (=  ?timel  ?start) ) 

?point2  <-  (Point  (time  ?time2)‘  (x  ?x2)  (y  7y2)  (z  ?z2)) 

(test  (=  7time2  ?end) ) 

(test  (>■  (distance  ?xl  ?yl  ?zl  ?x2  ?y2  ?z2)  2.0))  ;  skip  short  segments 

(bind  ?deltal  (normalize2  (-  ?orientation  0.0))) 

(bind  ?delta2  (normalize2  (-  ?orientation  90.0))) 

(bind  ?delta3  (normalize2  (-  ?orientation  180.0))) 

(bind  7delta4  (normalize2  (-  ?orientation  270.0))) 

(if  (<  (abs  ?deltal)  (min  (abs  ?delta2)  (abs  7delta3)  (abs  7delta4)))  then 
(bind  ?*newgyroerror*  ?deltal) ) 

(if  (<  (abs  7delta2)  (min  (abs  ?deltal)  (abs  7delta3)  (abs  7delta4)))  then 
(bind  ?*newgyroerror*  7delta2) ) 

(if  (<  (abs  7delta3)  (min  (abs  7delta2)  (abs  ?deltal)  (abs  7delta4)))  then 
(bind  ?*newgyroerror*  7delta3)) 

(if  (<  (abs  7delta4)  (min  (abs  7delta2)  (abs  7delta3)  (ab8  ?deltal)))  then 
(bind  ?*newgyroerror*  7delta4)) 

(bind  7*gyroerrortime*  (avg  ?start  7end) )  ;  average  time  of  wall  segment 
(format  t  "%nUser-provided  gyro  error  »  %4. If  degrees"  ?*gyroerror*) 

(format  t  "InWall  orientation  -  %4.1f  degrees"  ?orlentatlon) 

(format  t  "  for  time  %3.1f  (%3.1f  ..  %3.1f)« 

(avg  ?start  ?end)  ?start  ?ond) 

(format  t  "InExpert  system  gyro  error  ■  %4.1f  degrees"  ?*newgyroerror*) 


(dofrule  dotermine^gyro-drift-rate 
(declare  (salience  300)) 

?pool  <-  (location  pool)  ;  this  rule  only  works  in  the  pool 
?poly  <-  (Polyhedron  (classification  WALL)  (3tart  ?polystart)  (end  ?polyend) 
(status  COMPLETE) ) 

(test  (<>  ?*newgyroorror*  0.0))  ;  perform  only  if  new  gyro  error  calculated 
?line  <-  (Regression  line  (start  ?3tart) (end  7end) (orientation  ?orientation) 

(status  USED  |  USED  F0R_AREA) ) 

(test  (»  ?polyend  ?end) ) 

-> 

(bind  ?deltal  (normalize2  (-  ?orientation  7*newgyroerror*  0.0  ))) 

(bind  7delta2  (normalize2  {-  7orientation  7*newgyroerror*  90.0))) 

(bind  7delta3  (normallze2  (-  ?orientation  ?*newgyroerror*  180.0))) 

(bind  7delta4  (normalize2  (-  ?orientation  ?*newgyroorror*  270.0))) 

(if  (<  (abs  ?deltal)  (min  (abs  7delta2)  (abs  7delta3)  (abs  7delta4)))  then 
(bind  7*newgyrodriftrate*  ?deltal)) 

(if  (<  (abs  7delta2)  (min  (abs  ?deltal)  (abs  7delta3)  (abs  7delta4)))  then 
(bind  ?*newgyrodriftrate*  7delta2)) 

(if  (<  (abs  7delta3)  (min  (ab3  7delta2)  (abs  ?deltal)  (abs  7delta4)))  then 
(bind  ?*newgyrodriftrate*  7delta3)) 

(if  (<  (abs  7delta4)  (min  (abs  7delta2)  (abs  7delta3)  (abs  ?deltal)))  then 
(bind  ?*newgyrodriftrate*  7delta4)) 

(format  t  "%nWall  orientation  »  %4.1f  degrees"  ?orientation) 

(format  t  "  for  time  %3.1f  (%3.1f  ..  %3.1f)" 

(avg  ?start  ?end)  ?start  ?end) 

(format  t  "%nCurrent  gyro  error  =  %4.1f  degrees%n" 

(+  ?*newgyrodriftrate*  ?*newgyroerror*) ) 

(bind  ?*newgyrodriftrate*  (*  3600.0  (/  ?*newgyrodriftrate* 

(-  (avg  ?start  ?end)  ?*gyroerrortime*) ) ) ) 


) 


(if  (<=  (abs  ?*newgyrodriftrate*)  200.0)  then 

(format  t  "Expert  system  gyro  drift  rate  =  %4.1f  degrees/hour  %n" 
?*newgyrodriftrate*) ) 


Completion! 

(defrule  plot-pool-graph-file-when-done  ;  this  rule  is  the  last  to  be  fired 
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(declare  (salience  0))  ;  all  other  rules  take  precedence 

?range-file'-closed  <-  (range-file-clo3ed-flag) 

(format. t 

"%n%nElapsed  time. to  perform  sonar  classification:  %3.if  seconds. %n%h" 
(-  (time)  ?*time*j ) 

;  all  file  outputs  complete 
(close  plotfile) 

(close  auvfile) 

(printout  t  crlf  crlf  "Sending  pool.auv  to  iris  graphics  subdirectory." 
crlf) 

;  first  save  old  copy  of  file  to  pool.bak 

(printout  t  crlf  "rep  gemini:~brutzman/clips/pool.auv.bak" 

"  irisl:~brutzman/graphics/pool.auv.bak"'  crlf) 

(system  "rep  gemini:~brutzman/clips/pool. auv. bale" 

"  irisl : -brutzman/graphics/pool . auv. bak") 

(printout  t  crlf  "rep  gemini:~brutzman/clips/pool.auv" 

"  irisl: -brutzman/graphics/pool. auv"  crlf) 

(system  "rep  gemini:~brutzman/clips/pool.auv" 

"  irisl : -brutzman/graphics/pool. auv") 

(printout  t  crlf  "rep  gemini:~brutzman/clips/pool.auv" 

"  lrisl:~brutzman/preview/pool.auv"  crlf) 

(system  "rep  gemini:~brutzman/clips/pool.auv" 

"  irisl : -brutzman/preview/pool . auv") 

;  You  must  be  running  under  sunview  on  a  workstation  for  sunplot  to  work, 
(printout  t  crlf  crlf  "The  generated  pool. graph  sunplot  follows:"  crlf  crlf) 

(printout  t  "graph  -b  -g  1  -1  \"NPS  AUV  Sonar  Classification  Expert  System  " 
"\"  -x  145  -15  -y  -50  110  <  pool. graph  |  sunplot  -c  650" 

crlf  crlf) 

(system  "graph  -b  -g  1  -1  \"NPS  AUV  Sonar  Classification  Expert  System  " 
"\"  -x  145  -15  -y  -50  110  <  pool. graph  |  sunplot  -c  650") 
(system  "rm  core")  ;  remove  core  dump  file  which  resulted 
;  if  not  running  under  sunview 

(if  (yes-or-no  "Do  you  want  to  print  the  screen  log  file") 
then  (dribble-off) 

(system  "enscript  -G  -r  auvsonar.log")) 

(if  (yes-or-no  "Do  you  want  a  hard  copy  of  the  sonar  plot") 
then 

(open  "pool.auv"  auvfile  "a") 

(open  "pool. graph"  plotfile  "a") 

(printout  t  crlf) 

(if  (ye3-or-no  "Do  you  want  to  add  a  comment  line  tc  the  plot") 
then  (printout  t  crlf  crlf  "Enter  comment:  ") 

(bind  ?comments  (readline)) 

(printout  auvfile  crlf  "Comment:  "  Tcomments  crlf) 

(printout  plotfile  "  115  80  "  crlf  "\""  ’comments  »  \"  "  crlf)) 

(printout  t  crlf  crlf  "The  generated  pool. graph  is  being  plotted:" 
crlf  crlf) 

(printout  t  "graph  -b  -g  1  -1  V'NPS  AUV  Sonar  Classification  Expert  System  " 
-x  145  -15  -y  -50  110  <  pool. graph  (  lpr  -g  -h  -Pap2" 

crlf  crlf) 

(system  "graph  -b  -g  1  -1  V'NPS  AUV  Sonar  Classification  Expert  System  " 
"\"  -x  145  -15  -y  -50  110  <  pool. graph  I  lpr  -g  -h  -Pap2") 

;  all  file  outputs  complete 
(close  plotfile) 

(close  auvfile) 


(printout  t  crlf  "The  generated  pool.auv  file  follows:"  crlf  crlf) 
(system  "more  pool.auv") 

(printout  t  crlf  crlf  crlf) 


Polyhedron  output  function  and  rules 


(def function  output_polyhedron  (Pstarttime  ?endtime 

?startx  Tstarty  ?startz 

?endx  ?endy  ?endz 

?classification  Tcomment) 
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(format  t  ".%h%riThe  polyhedron  at  times  (%3.1f  ..  %3.1f)  ." 

Pstarttime  . Pendtime) 

(printout  t  "has’ classification  "  Pclassification  crlf  crlf) 
(format  auvfile 

"%n%s  %5.1f  %4.1f  %3;lf  %5.1f  %4.1f  %3.1f  time  %4.1f  %s" 

Pclassification 
(+  Pstartx  ?*offsetx*) 

(+  ?starty  ?*offsety*j 
(+  ?startz  ?*offsetz*) 

(+  ?endx  ?*offsetx*) 

(+  ?endy  ?*offsety*) 

(f  ?endz  ?*offsetz*) 

Pendtime 

?comment) 

(format  auvfile  "%n") 

(format  ?*out*  "%n%n") 

(format  ?*out* 

•!%n%s  %5.1f  %4.1f  %3. If  %S.lf  »4.1f  %3.1f  time  %4.1f  %s" 

?classification 
(+  ?startx  ?*offsetx*) 

(+  ?starty  ?*offsety*j 
(+  ?startz  ?*offsetz*) 

(+  ?endx  ?*offsetx*) 

(+  ?endy  ?*offsety*) 

(+  ?endz  ?*offsetz*) 

?endtime 

?comment) 

(format  ?*out*  "%n%n") 


) 


(defrule  change-colors-for-inferred-edges-when-done 


«> 


) 


(declare  (salience  40))  ;  pre-completion  rules  take  precedence 

?range-file-closed  <-  (range-file-closed-flag) 

(printout  auvfile  crlf  crlf  ?*color2*  "  Color  scheme  for  inferred  edges  " 
crlf)  ;  secondary  default  color  scheme 


(defrule  output-polyhedrons-with-inferred-edges-when-done 

(declare  (salience  30))  ;  pre-completion  rules  take  precedence 

?range-file-closed  <-  (range-file-closed-flag) 

?poly  <-  (Polyhedron  (status  COMPLETE  |  USED  FOR_AREA) 

(start  ?startpolytimeT  ~ 

(end  ?endpolytime) 

(startx  ?startx)  (starty  ?starty)  (startz  ?startz) 
(trait  INFERRED_EDGE) 

(classification  WALL)) 

;  node  matches  end  of  polyhedron 

?node  <-  (Node  (time  Pnodetime)  (x  Pnodex)  (y  Pnodey)  (z  Pnodez)) 

(test  («  Pendpolytime  Pnodetime) ) 


=> 

(output_polyhedron 


) 


Pstartpolytime  Pendpolytime 
Pstartx  Pstarty  0.0 
Pnodex  Pnodey  8.0 
"WALL" 

"(inferred  edge)") 


(defrule  change-colors- for-hidden-edges-when-done 


) 


(declare  (salience  20))  ;  pre-completion  rules  take  precedence 

Prange-file-closed  <-  (range-file-closed-flag) 

(printout  auvfile  crlf  crlf  P*color3*  "  Color  scheme  for  hidden  edges  " 
crlf)  ;  tertiary  default  color  scheme 
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(defrule  output-object-polyhedrons-with-hidden-edges-when-done 

(declare  (salience  10) )  ;  pre-completion  rules  take  precedence 

Trange-file-closed  <-  (range-fiie-closed-flag) 

?poly  <-  (Polyhedron  (status -COMPLETE  |  USED  FOR_AREA) 

(start  TstartpolytimeT 
(end  ?endpolytlme) 

(startx  Tstartx)  (starty  ?starty)  (startz  ?startz) 
(trait  HIDDEN_EDGE) . 

(classification  WALL)) 

;•  node  matches,  end -of  polyhedron 

?node  <-  (Node  (time  ?nodetime)  (x  ?nodex)  (y  ?nodey)  (z  Tnodez)) 

(test  (=  Tendpolytime  Tnodetlme) ) 


(output_polyhedron  ?startpolytime  ?endpolytime 
~  ?3tartx  Tstarty  0.0 

?nodex  ?nodey  8;0 
"WALL" 

"(hidden  edge)") 


Polyhedron  building  rules 


(defrule  polyhedron-building-start 

(declare  (salience  430) )  ;  lower  salience  value  than  polyhedron-building 

?line  <-  (Regression_line  (status  VALID) 

(start  ?starttlme)  (end  ?endtime) ) 

?start-node  <-  (Node  (time  ?nodetime) 

(accuracy  ?accuracyl) 

(x  ?startx)  (y  Tstarty)  (z  Tstartz)) 

(test  (=  Tstarttime  Tnodetime) ) 

Tend-node  <-  (Node  (time  Tendnodetime) 

(accuracy  ?accuracy2) 

(x  ?endx)  (y  ?endy)  (z  ?endz) ) 

(test  (*»  Tendtime  Tendnodetime)) 

"> 

(assert  (Polyhedron  (status  ACTIVE) 

(start  Tstarttime)  (end  Tendtime) 

(startx  Tstartx)  (starty  Tstarty)  (startz  Tstartz) 
(centroidx  »(+  Tstartx  Tendx) ) 

(centroidy  «(+  Tstarty  Tendy) ) 

(centroidz  **(+  Tstartz  Tendz)) 

(sidecount  1) 

(accuracy  “(max  Taccuracyl  Taccuracy2)) 

(trait  OBJECT_BUILDING_BASED) 

(classification  WALL))) 

(modify  Tline  (status  USED)) 

(bind  Tlength  (distance  Tstartx  Tstarty  Tstartz  Tendx  Tendy  Tendz) ) 

(if  (>»  Tlength  T*min_wall_length*) 
then  (output_polyhedron  Tstarttime  Tendtime 

Tstartx  Tstarty  0.0 
Tendx  Tendy  8.0 
WALL  "(new  polyhedron  start)")) 


) 


(defrule  polyhedron-building-continuation 

This  rule  tests  the  newest  edge  in  relation  to  che  most  recent  previous  edge 
of  the  currently  ACTIVE  polyhedron. 

If  the  edges  are  too  far  apart,  the  previous  polyhedron  is  COMPLETE  and  the 
new  edge  is  ignored  in  order  for  it  to  begin  a  new  polyhedron. 

If  the  edges  are  colinear,  the  new  edge  is  included  as  part  of  the 
currently  ACTIVE  polyhedron. 

;  If  the  edges  are  conca; e,  the  previous  polyhedron  is  COMPLETE  and  the  new 
;  edge  is  ignored  in  order  to  let  it  begin  a  new  polyhedron. 
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;  If  the  edges  are  convex,  the  new  edge  is  included  as  part  of  the' 

;  currently  ACTIVE  polyhedron,  and  the  polyhedron  is  reclassified 

;  from  HALL  to  OBJECT. 

;  Specific  polyhedron  OBJECT  reclassifications  will  be  made  by.  higher  level  rules. 

(declare  (salience  440})  ;  higher  salience  value  than  polyhedron  start 
?poly  <-  (Polyhedron  (status  ACTIVE) 

(start  ?startpolytime). 

(end  Tendpolytime) 

(accuracy  Tpolyaccuracy) 

(startx  ?startx) 

(starty  ?starty) 

(startz  Tstartz) 

(centroidx  ?centroidx) 

(centroidy  ?centroidy) 

(centroidz  ’centroidz) 

(sidecount  Tsidecount) 

(area  '  ?area) 

(classification  Pclassification) ) 

;  lihel  is  most  recent  valid  regression  line  included  in  the  polyhedron 
?linel  <-  (Regression_line  (status  USED) 

(start  ?startlineltime) 

(end  ?endlineltime) 

. (orientation  ?orientationl) ) 

(test  (=  ?endpolytime  ?endlineltime) ) 

;  line2  is  newest  valid  regression  line  to  be  evaluated 
?line2  <-  (Regression_line  (status  VALID) 

(start  ?startline2time) 

(end  ?endline2time) 

(orientation  ?orientation2) j 

;  nodel  matches  end  of  linel  (most  recent  valid  regression  line) 

?nodel  <-  (Node  (time  ?nodeltime) 

(accuracy  ?accuracy2) 

(x  ?nodelx)  (y  ?nodely)  (z  ?nodelz) ) 

(test  (=  ?endlineltime  ?nodeltime) ) 

;  node2  matches  start  of  line2 

?node2  <-  (Node  (time  ?node2time) 

(accuracy  ?accuracyl) 

(x  ?node2x)  (y  ?node2y)  (z  ?node2z) ) 

(test  (»  ?3tartline2time  ?node2time>) 

;  node3  matches  end  of  line2 

?node3  <-  (Node  (time  ?node3time) 

(accuracy  ?accuracy3) 

(x  ?node3x)  (y  ?node3y)  (z  ?node3z) ) 

(test  ("  ?endline2tirae  ?node3time) ) 


(bind  ?distance  (distance  ?nodelx  ?nodely  ?nodelz  ?node2x  ?node2y  ?node2z) ) 

;  if  distance  is  too  great,  don't  continue  building  polyhedron  with  new  edge 
(if  (>  ?distance  ?*max_edge_distance*) 
then  (modify  ?poly  (status  COMPLETE) 

(area  “(abs  ?area)) 

(centroidx  »(/  ?centroidx  ?sidecount  2))  ;  2  points/side 

(centroidy  =(/  ?centroidy  ?sidecount  2)) 

(centroidz  •>(/  ?centroidz  ?sidecount  2)) 

(sidecounterl  ?sidecount) 

(sidecounter2  ?sidecount)) 

;  if  polyhedron  was  not  a  WALL,  assert  a  HIDDEN  EDGE  wall  for  it 
(if  (not  (eq  ?classification  WALL)) 
then  (format  t  "%nPolyhedron  OBJECT  (%3.1f  ..  %3.1f)  " 

?startpolytime  ?endpolytime) 

(format  t  "has  area  %3.1f%n"  (abs  ?area)) 

(format  auvfile  "  (prior  object  area  was  %3.1f)"  (abs  ?area) ) 
(assert  (Polyhedron  (status  COMPLETE) 

(start  ?startpolytime) 

(end  Tendpolytime) 

(startx  ?startx) 

(starty  ?starty) 

(startz  ?startz) 

(centroidx  =(avg  ?startx  ?node3x)) 

(centroidy  =(avg  ?starty  ?node3y) ) 

(centroidz  =(avg  ’startz  ?node3z)) 

(sidecount  1) 

(accuracy  ?polyaccuracy) 
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'  (trait  HIDDEN_EDGE) 

(classification  HALL) ) ) ) 

) 

(bind  Jlength  (distance  ?node2x  ?node2y  ?node2z  ?node3x  ?node3y  ?node3z) ) 

if  distance  is  close,  enough,  then- test  colinear/convex/coricave 
(if  (<=  ^distance  ?*max_edge_distance*) 

then  (if  (<=  (abs  (riormalize2  (-  ?orientationl  ?orientation2) ) ) 
?*max_edge_angle*) 

then  ;  colinear” edge  found  and  added  to  polyhedron 

;  also  add  'S'  area  between  start  point  and  new  segments 
(bind  ?triangleareal  (S  ?startx  ?starty 

?nodelx  Jnodely  ?node2x  ?node2y) ) 

(bind  ?trianglearea2  (S  ?startx  ?starty~ 

?node2x  ?node2y  ?node3x  ?node3y) ) 

(modify  ?poly  (end  ?endline2time) 

(area  =(+  ?area  Jtriangleareal  ?trianglearea2)_) 
(centroidx  =(+  ?centroidx  ?node2x  ?nbde3x)) 

(centroidy  =(+  ?centroidy  ?node2y  ?node3y) ) 

(centroidz  =(+  Jcentroidz  ,?node2z  ?node3z) ) 

(sidecount  =(+  ?side*count  1))) 

(modify  ?line2  (status  USED) ) 

(assert  (Polyhedron  (status  COMPLETE) 

(start  ?endlineltime) 

(end  ?startline2time) 

(startx  ?nodelx) 

(starty  ?nodely) 

(startz  ?nodelz). 

'(centroidx  =(avg  ?nodelx  ?node2x)) 

(centroidy  -(avg  ?nodely  ?node2y) ) 

(centroidz  -(avg  Tnodelz  ?node2z) ) 

(sidecount  1)= 

(accuracy  -(max  ?accuracyl  ?accuracy2)) 

(trait  INFERRED_EDGE) 

(classification  WALL) )  )- 
(if  (>=  ?length  ?*mln_wall_length*) 
then  (output_polyhedron  ?startline2time  ?endline2time 

?node2x  ?node2y  0.6 
?node3x  ?node3y  8.0 
"WALL" 

"(added  colinear  edge)")) 
else  ;  test  for  convex  edge  to  continue  building, 

;  otherwise  edge  is  concave  and  polyhedron  is  complete. 

;  note  this  rule  currently  coded  to  work  only  for  left  transducer 

(if  (<  (normalize2  (-  ?orientation2  ?orientationl) )  0.0) 

then  convex  edge  found,  and  joined  to  polyhedron 

;  also  add  ' S'  area  between  start  point  and  new  segments 
(bind  ?triangleareal  (S  ,?startx  ?starty 

?nodelx  ?nodely  ?node2x  ?node2y) ) 
(bind  ?trianglaarea2  ^S  ?startx  ?starty 

?node2x  ?node2y  ?node3x  ?node3y) ) 
(modify  ?poly  (end  ?endline2time) 

(classification  OBJECT) 

(area  -(+  ?area  ?triangleareal 

?trianglearea2) ) 
(accuracy  -(max  ?accuracy2 

?accuracy3  ?polyaqcuracy) ) 
(centroidx  =(+  ?centroidx  ?node2x  ?node3x) ) 

(centroidy  =(+  ?centroidy  ?node2y  ?node3y) ) 

(centroidz  -(+  ?centroidz  ?node2z  ?node3z)) 

(sidecount  =(+  ?sidecount  1))) 

(modify  ?line2  (status  USED) ) 

(assert  (Polyhedron  (status  COMPLETE) 

(start  ?endlineltime) 

(end  ?startline2time) 

(startx  ?nodelx) 

(starty  ?nodely) 

(startz  ?nodelz) 

(centroidx  =(avg  ?nodelx  ?node2x)) 
(centroidy  =(avg  ?nodely  ?node2y)) 
(centroidz  =(avg  ?nodelz  ?node2z)) 
(sidecount  1) 

(accuracy  =(max  ?accuracyl  ?accuracy2)) 
(trait  INFERRED_EDGE) 

(classification  WALL))T 
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(if  (>=  Plength  ?*min_wall_lehgth*) 
then  (output_polyhedron  Pstartline2tirae:  Pendline2time 

Pnode2x  ?node2y  0.0 
?node3x  Pnode3y  8.0 
"WALL" 

“ (added- convex  edge) "j ) 

else  ;  concave  edge  found  so  don't  continue  building  polyhedron 
(modify  ?poly  (status  COMPLETE) 

(area  =  (abs  Parea)). 

(centroidx  =(/  Pcentroidx  Psidecount  2)) 

(centroidy  =  {/  Pcentroidy  Psidecount  2)) 

(centroidz  ={/  Pcentroidz  Psidecount  2)) 

(sidecounterl  Psidecount) 

(sidecounter2  Psidecount)) 

;  if  polyhedron  was  not  a  WALL,  assert  a  HIDDEN_EDGE  wall 
(if  (not  (eq  Pclassification  WALL) ) 
then  (format  t  "%nPolyhedron  OBJECT  (%3.1f  ..  %3.1f)  " 
Pstartpolytime  Pendpolytime) 

(format  t  "has  area  %3.1f%n"  (abs  Parea)) 

(format  auvfile  "  (prior  object  area  was  %3.1f)n  (abs 

Parea) ) 

(assert  (Polyhedron  (status  COMPLETE) 

(start  Pstartpolytime) 

(end  Pendpolytime) 

(startx  Pstartx) 

(starty  Pstarty) 

(startz  Pstartz) 

(centroidx  ={avg  Pstartx  Pnode3x)) 

(centroidy  =(avg  Pstarty  Pnode3y) ) 

(centroidz  >=(avg  pstartz  ?node3z) ) 

(sidecount  1) 

(accuracy  Ppolyaccuracy) 

(trait  HIDDENJEDGE) 

(classification  WALL)7)) 

))) 

) 


(defrule  complete-active-polyhedron-after-file-reading-finished 

(declare  (salience  420) )  ;  polyhedron  determination  rules  take  precedence 

Prange-file-closed  <-  (range-file-closed-flag) 

Ppoly  <-  (Polyhedron  (status  ACTIVE) 

(start  Pstartpolytime) 

(end  Pendpolytime) 

(startx  Pstartx) 

(starty  Pstarty) 

(startz  Pstartz) 

(accuracy  Ppolyaccuracy) 

(centroidx  Pcentroidx) 

(centroidy  Pcentroidy) 

(centroidz  Pcentroidz) 

(sidecount  Psidecount) 

(area  Parea) 

(classification  Pclassification) ) 

;  node  matches  end  of  polyhedron 
Pnode  <-  (Node  (time  Pnodetime) 

(accuracy  Paccuracy) 

(x  Pnodex)  (y  Pnodey)  (z  Pnodez) ) 

(test  (-  Pendpolytime  Pnodetime)) 


(modify  Ppoly  (status  COMPLETE) 

(area  =(abs  Parea) ) 

(centroidx  =(/  Pcentroidx  Psidecount  2))  ;  2  points/side 
(centroidy  =(/  Pcentroidy  Psidecount  2)) 

(centroidz  =(/  Pcentroidz  Psidecount  2)) 

(sidecounterl  Psidecount) 

(sidecounter2  Psidecount)) 

'  ;  if  polyhedron  was  not  a  WALL,  assert  a  HIDDEN_EDGE  wall  for  it 

(if  (not  (eq  Pclassification  WALL)) 

then  (format  t  "%nPolyhedron  OBJECT  (%3.1f  ..  %3.1f)  has  area  %3.1f%n" 
Pstartpolytime  Pendpolytime  (abs  Parea)) 

(format  auvfile  "  (prior  object  area  was  %3.1f)"  (abs  Parea)) 
j  (assert  (Polyhedron  (status  COMPLETE) 

(start  Pstartpolytime) 
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(end  ?endpolytiipe) 

(startx  ’startx) 

(starty  Tstarty) 

(starts  Tstartz) 

{centroidx  - (avg  ’startx  ’nodex) ) 

(centroidy  =  (avg  ?starty  Pnodey)) 

(centroidz  = (avg  ?startz  Pnodez) ) 

(sidecount  1) 

(accuracy  ?polyaccuracy) 

(trait  HIDDENJEDGE) 

(classification  :<ALL)7)) 


Completed  polyhedron  area  calculation  rules 


(defrule  oldareal 

f 

;  compute-polyhedron-area-contribution- from- regression-edges 

(declare  (salience  415) )  ;  polyhedron  determination  rules  take  precedence 

Ppoly  <-  ^Polyhedron  (status  COMPLETE) 

(trait  OBJECT_BUILDING_BASED) 

(classification  OBJECT) 

(start  Tstartpolytime) 

(end  ?endpolytime) 

(startx  ?startx) 

(starty  Tstarty) 

(startz  ?startz) 

(centroidx  ?node3x) 

(centroidy  ?node3y) 

(centroidz  ?node3z) 

(sidecount  ?sidecount)- 

(sidecounterl  ’sidecounterl) 

(area  ?area) ) 

(test  (>  ?sidecounterl  0))  ;  prevent  infinite  recursion 

;  get  the  next  line  contributing  to  polyhedron  area 
?edge  <-  (Edge  (start  ?startedgetime) 

(end  ?endedgetime) 

(status  USED)) 

(test  (and  (>=»  ?startedgetime  ?startpolytime) 

(<=  ?endedgetime  ?'endpolytime) ) ) 

;  nodel  matches  start  of  edge 

?nodel  <-  (Node  (time  ?nodeltime) 

(x  ?nodelx)  (y  ?nodely)  (z  Tnodelz) ) 

(test  (=  ?startedgetime  ?nodeltime)) 

;  node2  matches  end  of  edge 

?node2  <-  (Node  (time  ?node2time) 

(x  ?node2x)  (y  ?node2y)  (z  ?node2z) ) 

(test  (=  ?endedgetimo  ?node2time)) 


(bind  ?trianglearea  (S  ?nodelx  Tnodely  ?node2x  ?node2y  ?node3x  ?node3y)) 

(modify  ?poly  (sidecounterl  *•(-  ?sidecounterl  1)) 

(area  *>(*  ?area  (abs  ?trianglearea) ) ) ) 

(modify  ?edge  (status  USED_FOR_AREA) ) 


(defrule  oldarea2 

;  corapute-polyhedron-area-contribution-from-inferred-walls 

(declare  (salience  410))  ;  polyhedron  determination  rules  take  precedence 

?poly  <-  (Polyhedron  (status  COMPLETE) 

(trait  OBJECT  BUILDING_BASED) 

(classification  OBJECrT 
(start  ?startpolytime) 

(end  ?endpolytime) 

(startx  ?startx) 

(starty  ?starty) 
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(startz  Pstartz) 

{centroidx  Pnode3x) 

(centroidy  Pnode3y) 

(centroidz  Pnode3z) 

(sidecount  Psidecount) 

(sidecounter2  ?iidecounter2) 

(area  Parea) ) 

(test  (>  Psidecbunter2  0) )-  ;  prevent  infinite  recursion 

;  get  a  matching  inferred  wall,  or  hidden  edge  polyhedron 
?ooly2  <-  (Polyhedron  (status  COMPLETE) 

(trait  INFERRED_EDGE  I  HIDDEN_EDG2) 

(classification  HALL) 

(start  ?staftp'oly2tine) 

(end  Pehdpoly2time) 

(startx  Pnodelx) 

(starty  Pnodely) 

(startz  Pnodelz)) 

(test  (and  (>=  Pstartpoly2time  Pstartpolytime) 

{<=  Pendpoly2time  Pendpolytime) ) ) 

;  node2  matches  the  end  of  this  inferred/hidden  wall 
Pnode2  <-  (Node  (time  ?node2time) 

(x  ?node2x)  (y  ?node2y)  (z  ?node2z) ) 

(test  (=  ?endpoly2time  ?node2time) ) 


(bind  Ptrianglearea  (S  Pnodelx  Pnodely  Pnode2x  Pnode2y  Pnode3x  Pnode3y) ) 

(modify  Ppoly  (sidecounter2  =(-  Psidecounter2  1)) 

(area  «(+  Parea  (abs  Ptrianglearea) ) ) ) 

(modify  Ppoly2  (status  USED_FOR_AREA) ) 

(if  (eq  Psidecounter2  1)  ;  last  edge  triangle  has  been  added 
then  (format  t  "%nPolyhedron  OBJECT  (%3.1f  ..  %3.1f)  has  area  %3.1f%n" 

Pstartpolytime  Pendpolytime  (+  Parea  (abs  Ptrianglearea)))) 


Object  classification  rules:  the  top  level  at  last! 


(defrule  class! fy-pool-objects 

(declare  (salience  400))  ;  polyhedron  determination  rules  take  precedence 

Ppoly  <-  (Polyhedron  (status  COMPLETE) 

(trait  .  OBJECT_BUILDING_BASED) 

(classification  OBJECtT 
(start  Pstartpolytime) 

(end  Pendpolytime) 

(startx  Pstartx) 

(starty  Pstarty) 

(startz  Pstartz) 

(centroidx  Pcentroidx) 

(centroidy  Pcentroidy) 

(centroidz  Pcentroidz) 

(sidecount  Psidecount) 

(area  Parea) ) 

;  node  matches  end  of  polyhedron 
Pnode  <-  (Node  (time  Pnodetime) 

(accuracy  Paccuracy) 

(x  Pendx)  (y  Pendy)  (z  Pendz) ) 

(test  (=  Pendpolytime  Pnodetime) ) 


;  Reclassify  long  skinny  objects  as  walls 

(bind  Plength  (distance  Pstartx  Pstarty  Pstartz  Per.dx  Pendy  Pendz) ) 

(if  (<«  (/  Parea  Plength  Plength)  ?*wall_thinness_ratlo*) 
then  (bind  Parea  0.0) 

(modify  Ppoly  (classification  WALL)  (area  0.0)) 

(format  t  "%n*»*  OBJECT  at  (%3.1f  ..  %3.1£)  reclassified  as  a  WALL. %n" 
Pstartpolytime  Pendpolytime) 
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Mine  classification 

(if  (and  (>=  ?area  10.0)  (<=  ?area  .100.0) )  ;  area  criteria  test 
then  (modify  ?poly  (classification  MINE)) 

(format  t  ".%n%hThe  polyhedron  at  times  (%3.1f  ...  %3.1f)  " 
Jstartpolytime  ?endpolytime) 

(printout  t  "has  classification  MINE."  crlf  crlfj 
(format  auvfile 

"%n%s  %5.1f  %4,lf  %3.1f  %5.1‘f  time  %4.1f" 

MINE 

(+  ?centroidx  ?*offsetx*) 

(+  ?centroidy  ?*offsety*) 

(+  ?centroidz  ?*offsetz*) 

(/  ?area  (pi)  2  6);  radius 
?endpolytime) 

(format  auvfile  "in'?) 

(format  ?*out* 

"%n%s  %5. If  %4.1f  %3.1f  %S.lf  time  %4.1f" 

Mine 

(+  ?centroidx  ?*offsetx*) 

(+  ?centroidy  ?*offsety*) 

(+  ?centroidz  ?*offsetz*) 

(/  ?area  (pi)  2  .6)  ;  radius  t  sonar  beamwidth  fudge  factor 
?endpolytime) 

(format  ?*out*  "%n")) 
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Abstract 

This  paper  discussed  the  shortest  path  planning  problem  in  a  world  with  n  circular  obsta¬ 
cles.  This  problem  can  be  considered  a  simplified  mathematical  model  of  the  shortest  path 
planning  problem  for  a  polygonal  world.  As  we  established  a  method  to  treat  circles  in  a  man¬ 
ner  similar  to  nodes  in  a  search  graph,  the  Dijkstra’s  algorithm  can  be  applied  and  the  problem 
is  solved  in  time  0(?i2logn).  The  A*  algorithm  dramatically  improves  searching  efficiency  in 
this  problem.  These  results  are  given  with  numerous  example  figures.  However,  considering 
the  0(n 2)  time  result  on  the  n  line  segment  visibility  problem,  there  is  a  strong  possibility  of 
being  able  to  solve  this  circle  world  problem  in  0(n2)  time  also. 


1  Introduction 


The  shortest  path  problem  in  a  polygonal  world  has  been  extensively  discussed  in  numerous  articles. 
Let  n  be  the  number  of  segments  in  a  polygonal  environment.  Sharir  and  Shorr  first  showed  that  it 
is  solved  in  time  0(n2log?z)  [16]  and  this  was  improved  by  Welzl  [17]  and  Asano  et.al.  [2]  to  0(n 2). 
The  use  of  Dijkstra’s  algorithm  to  spatial  planning  problems  was  first  proposed  by  Lozzano-Perez 
and  Wesley  [10]. 

This  paper  discusses  the  shortest  path  planning  problem  in  a  world  with  n  circular  obstacles, 
which  first  appeared  in  [12].  This  problem  can  be  considered  a  simplified  mathematical  model  of  the 
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shortest  path  planning  problem  for  a  polygonal,  world.  As  is  well  known,  the  fundamental  approach 
to, solve  the  shortest  path  problemds  to  use  .visible  tangents  to  obstacles  [6,  3, 16, 15, 17, 2, 12,  9,  7]. 
This  paper  also  uses  that  approach.  One  of  the  significant  results  of  this  paper  is  that  we  are  able 
to  treat  a  circle  in  this  world  like  a  node  in  a  search  graph,  and  thus  are  able  to  employ  Dijkstra’s 
algorithm. 

At  least  one  previous  application,  the  Stanford  cart  mobile  robot,  used  a.  similar  circle  world 
model,  for  obstacle  representation  and.  avoidance.  The  combination  of  vision  processing,  and  path 
planning  aboard  that  small  robot  proved' prohibitive  for  real-time  use  due  to  hardware  limitations 
and  algorithmic  complexity  [12].  It  is  hoped  that  the  .optimal  algorithm  provided  in  this  paper  will- 
support  real-time  path  planning  by  autonomous  vehicles.  The  circle  world  search  model  is  directly 
extendible  to  the  general  case  of  three-dimensional-  path  planning  and  is  particularly  suitable  for 
underwater  vehicle  path  planning  [5]: 


2  Problem  Statements 


The  environment  for  this  path  planning.prpblem  .is.a.plane  on  which  a, global  Cartesian  coordinate 
system  is  defined.  A  circle  C  is  a  triple  of  XY  coordinates  and  a  radius  (>  0), 


C=(x,y,r). 


(1); 


A  world  W  is  a  set  of  circles, 

W  =  .(2) 

where  no  pair  of  circles  are  intersecting  or  touching.  A  free  space  is  a  complement  of  the  union  of 
all  the  circles  (the  free  space  includes  the  circular  boundaries).  Let  a  start  point  S  and  a  goal  point 
G  be  points  in  the  free  space.  Any  path  joining  S  and  G  must  stay  in  the  free  space.  The  problem 
is  to  find  a  shortest  path  joining  S  and  G  in  this  world  W  (Fig.  1). 


3  Fundamentals 

3.1  Tangents  and  Common  Tangents 

Let  C  =  (x,ytr)  be  a  circle,  If  the  intersection  of  a  directed  line  (or  a  ray)  and  the  circle  G  is 
only  a  point,  the  line  is  said  to  be  a  tangent  to  C.  If  the  circle  C  is  on  the  left  of  a  tangent  to  C, 
the  tangent  is  called  a  plus  or  counterclockwise  tangent.  If  C  is  on  the  right  of  a  tangent  to  (7,  the 
tangent  is  called  a  minus  or  clockwise  tangent  (Fig.  2).  The  “plus”  or  “minus”  characteristic  of  a 
tangent  is  called  the  mode. 
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Let  po  =  (xo,.yo)  be  a  point  outside  the  circle  C.  The  two  tangents,  from  po  to  C  are  uniquely 
determined  ;(Fig  2).  The  orientation  6+  of  the  plus  tangent  fropi  po  to.-the  circle  C  is 


(3) 


■where  stands  for  the  orientation  from  a  point  p  to  point  q  in  four  quadrants  ($(p,q)  £ 

,[— TTjTr]),  On  the  other  hand,  the  orientation  6~  of  the  minus  tangent  from  p0  to  the  circle  C  is 


0  ='H{po,{x,y))  +  sm-1(- 


r 


dist(pQ,(x,y)) 


(4) 


Therefore,  if  a  tangent  of  mode  m  from  a  point  to  a  circle  C,  assuming  that  its  value  is  +1  if  the 
mode  is  plus  and  -1  if  it  is  minus,  the  previous  two  equations  are  expressed  by 

6m  =  V(p0,(x,y))  -  -.)  =  V(po,(x,y))  -  (5) 


[  dist(po,(x,y)y 


v  disi(p0,(x,y))J 


The  intersection  of  a  tangent  and  a  circle  is  called- an  osculating  point.  In  this  case  in  Fig  2,  the 
osculating  points  are  also  called  landing  points. 


Likewise,  a  tangent  of  mode  m  from  a  circle  C  to  a  point  po  (Fig  3)  has  an  orientation 


0ni  =  ty((x,y),po)  +  sill-1  ( ) 

dist(po,.(x,y)) 

In  this  case,  the  osculating  points  are  called  leaving  points. 


(6) 


There  are  four  common  tangents  from  a  circle  C\  =  (*1 ,  y\ ,  ?'i)  to  another  C2  =  (^2, 2/2, ^2) 
(Fig.  4). 

Each  of  the  four  tangents  are  uniquely  named:  ++  tangent,  H —  tangent,  — 1-  tangent  and  ■ — 
tangent.  The  orientation  0mlm 2  of  these  tangents  is 


gm  1  m2 


^((^‘1,2/1), (*T2, 2/2))  +  sin  ]( 


ml  7’i  -  77i2  r2  v 
<li$t(p0){x,y)y 


(7) 


Hereafter  by  “tangent”  we  mean  a  directed  line  segment  between  a  point  and  osculating  point, 
or  between  osculating  points. 


3.2  Visibility 

The  test  for  intersection  of  a  (directed  or  undirected)  line  segment  L  with  a  circle  C  can  be  done  in 
a  constant  time  as  follows  (Fig  6).  First  we  find  the  region  in  Fig  6  that  the  center  of  C  is  located 
in.  After  that  we  can  compare  the  radius  of  C  with  d ,  where  d  is  the  distance  between  the  center 
of  C  and  L ,  or  the  distance  between  the  center  and  one  of  the  endpoints  of  L  (if  L  merely  osculates 
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the  circle,  it  is  not  considered  intersecting).  A , line  segment  L  in  a  world  W  is  visible  in  a  world 
W  if  L.  does  not  cross  the  circumference  of  any  circle  in  Basically,,  the  visibility  test  in  W  for  a 
line  segment  needs  0(h).  time.  Jjowever,  integrating  these  tests  simply  for  all  the  possible  tangents 
requires  0(n3)  time,  since  the  number  of  all  the  tangents  is  0(n2).  Thus,  we  will  use  some  other 
method  as  discussed  later. 


3.3  Characterization  of  Shortest  Path 

A  directed  circle  is  a  circle  C  with  a  mode  or  direction .  A  mode  m  is  either  counterclockwise  (or 
plus)  or  clockwise  (or  minus)';  A  part  of  a  directed  circle  can.be  used  as.  a  component  of  a  shortest 
path. 

With  a  world  of  W,  vye  define  the  set  M  =  M(W‘)  of  (tangent)  modes: 

M(W)  =  {C+,C~\C  e  W}  (8) 

A  mode  sequence  a  over  J\1  is  a  finite  sequence  of  modes,  such  that  no  subsequence- of  0+C+, 
-0+0“,  C~C+  or  C~C~  (with  a.  single  G  €  H7)  is  contained.  The. empty  mode  sequence 'is  denoted 
by  t.  The  set  of  all  the  mode  sequences  is  expressed  as  M(W)'  following  the  convention  of  language 
and  automata  theory.  If  W  =  .{0,0},  examples  of  tangent  sequences,  are: 

e,  0+.  O',  0+,  0",  0+0+,  0+0“,  •  •  • ,  0.-0+’,  •  •  • 


A  mode  sequence  a  6  M,(W)m  with  a  start  S  and  goal,  G  can  be  interpreted  into  a  path 
w(o)  by  the  following  two  rules.  Furthermore,  we  assume  the  same  tangent  mode  does  not  occur 
consecutively  in  a.  mode  sequence  a. 

(I)  If  <7=  e, 

7 r(a)  =  SG  if  visible(S,G)  (9) 

where  vi$ible(S,G)  means  “S  and  G  are  visible  in  this  world"  foi  directed  line  segment  SG.  If  they 
are  not  visible,  the  value  7r(e)  is  undefined. 

(II)  If  a  =  Cft1  ■  ■  C™9 ,  where  q >  1,  On,  •  •  • ,  0;9  €  W,  and  ml mq  e  {+,-}, 

7r(q)  =  lokilik2h.. .  kqlq  (10) 


where 


1.  the  right  hand  side  of  this  equation  is  the  concatenation  of  the  2 q  +  1  subpaths, 

2.  Iq  is  a  ml  tangent  from  S  to  circle  Cn  if  the  tangent  is  visible  (in  this  world), 
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3..  for  each  jf,(f  <j£  4^1),  lj  is  ( mjvrn('j  -H))'  common  tangent  from-circle  Ci/  to  circle  6$+i. 
if  this  tangent  is  visible, 

4.  /j.  is  a  mq  tangent  from  circle  C,-g  to  G/if  this  tangent  is  visible,  and 

5.  .‘for  each  j(  1  <  j  <, <q),  kj  is  the  minimum  portionof  the  m(zy)-directed  circle  G,j  between  the 
two  osculating  points  of  the  previous  and  next  tangents  (if  both  tangents  are  visible). 

6.  If  any  of  these  subpaths  donot  exist  in  the  world,  the  value  7r.(cr),  is  undefined's 

A  path  7r  is  called  canonical  if  there  exists  a  mode  sequence  a  such  that  7r  ==  i r(cr).  The  following 
proposition  is  essential  in  shortest  path  planning  problem: 


Proposition  3.1  The  shortest-  path  for  a  given  world  and  two  endpoints  is  canonical. 


4  Dijkstra’s  Algorithm 


Having  Proposition  3:1,  there  exists  the  possibility  of  applying  Dijkstra’s.  algorithm  to  this  path 
finding  problem.  Dijkstra’s  algorithm- is  a  standard  approach -to  the  single-source  shortest  path 
problem  in  a -graph  [1,  11].  Although' the  free  space  in  a  circle  world  has  much  more  freedom 
than  a  network  graph  in  path  planning,  we  can  treat  a  circle  as -a  “generalized”  search  node  and 
tangents  as  arcs  by  using  minor  search  modifications  . and  by  taking  advantage  of  Proposition  3:1. 
This  proposition  says  that  a-shortest  path  has- a  mode  sequence  d- and  lands  on  one.or  more  circles 
if  S  and  G  are  not  visible.  Therefore,  if, a  circle  Cm  is  in  a,  the  partial  path  from  51  to  Cm  is  the 
shortest  one  of  all  the  possible  paths  from  S  to  Cm.  The  essence  of  this  concept  is  the  same  as  that 
of  Dijkstra’s  algorithm.  A  relatively  complicated  question  is  how  two  paths  with  distinct  landing 
points  on  C  should  be  compared. 

The  following  visibility  preprocessing  is  required  prior  to  employing  Dijkstra’s  algorithm; 

1.  visible  tangents  from  S'  to  all  the  circles  in  IT. 

2.  visible  common  tangerns  between  all  the  circles  in  W. 

3.  visible  tangents  from  all  the  circles  in  IT  to  G. 

4.1  Preprocessing  for  Visible  Tangents  to  or  from  a  Point 

Let  us  consider  the  problem  of  finding  all  the  visible  tangents  from  S  to  all  the  circles  in  IT.  The 
other  problems  of  finding  tangents  between  circles  and  tangents  to  G  can  be  solved  in  a  similar 
manner. 
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Step  1.  Compute  ail  the  orientations  of  tangents;from  5  to  all  the  circles,  in.  both,  inodes.. 

Step  2.  Sort  these  orientations  using  a  heap. 

Step  3.  Sweep  all  the  tangent  by  their  orientations  starting  with  the  orientation  of  the  tangent 
with  the  shortest  length.  In  this,  sweep  process,  visibility  is  tested  using  a  heap  built  according  to 
tangent  lengths.  (For  sweep  technique,  see  [14].) 

The  computational  time  is  0(n)  for  Step  1,  0(n  log  n)  for  Step  2,  and  O(n  log  n)  for  Step  3  (n. 
is  the  number  of  circles).  Thus,  the  overall  computational  complexity  is  0(n  logn).  The  result  of 
applying  this  algorithm  to  the  world  given  in.  Figure  1  is  shown  in  Figure  7.  All  visible  tangents 
from  all  circles  to  G  are  shown,  in  Figure  8, 


4.2  Preprocessing  for  Visible  Common  Tangents 

There  are  two  obvious  ways  to  do  this  in  0(n2  log  n)  time.  One  is  to  compute  all  the  common 
tangents  from  a  circle  in  0(nlognj  time  (Fig  9)  and  repeat  this  for  all  n  circles.  Another  method 
is  to  list  orientations  of  all  the  common  tangents  among  circles.  This  task  also  needs  0(n2  log  n) 
time:  Next  we  sweep  these  tangents  by  orientation  to  update  the  visibility  relation  [14,  17]..  This 
part  again  takes  '0(u2  log  ??.).  time. 

However,  there  is  a  strong  possibility  of accomplishing  the.entire.process  in  0(n2)  time  using  the 
similar  methods  developed  for  finding,  a  visibility  graph  of  rc  line  segments  in  that  time  complexity 
[17,  2].  This, is  ah.  open  problem.  All  common  tangents  in  the  example  circle  world  are  shown  in 
Figure  10. 


4.3  Comparison  between  Two  Landing  Paths 

Figuie  11  shows  an  example  in  which  t\vo  paths  7rj  and,7r2;land  on  a  directed  circle.  In  addition  let 
A12  be  a  portion  of  the  directed  circle  from  the  landing  point  of  7rj  to  that  of  7r2.  A2j  is  defined  in 
an  analogous  manner.  The  union  of  both  portions  make  a  complete  directed  circle. 

Proposition  4.1  Assume  ttj  and  7r2  land  on  a  m-directed  circle  (m  €  {+.  —}).  Either  of  the 
following  cases  hold'. 

(Case  I)  A  closed  path  (tti,  A12, 7r2)  does  not  contain  the  circle. 

(Case  II)  A  closed  path  (tt2)!A 2i,tt1)  does  not  contain  the  circle. 

Let  lg( 7r)  denote  the  length  of  a  path  n. 

Proposition  4.2  In  the  situation  described  in  Proposition  f.l,  in  Case  I,  compare 

igM  +  W12)  and  kfa)  (11) 
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On  the  other  hand,  in  Case  II,  compare 

Igfa)  +  lg( A21)  and  lg{vi)  (12) 

In  each  case  the  longer  overall  path  should  be.  discarded. 

Proof.  Let  us  examine  the  situation  in  Figure  11  which  falls  under  Case  L  In  this  case,  we 
assume  the  goal  G  or  next  landing  point  is  in  region  L,  but  not  in  region  R' in  the  figure.  If  the  goal 
or  next  landing  point  is  in  region  R,  there  exists  a  better  path  than  7r2  and' its  use  is  meaningless. 
Therefore,  the  shorter  of  the  two  paths  in  Equation  11  is  better  for  further  extension.  □ 

In  order  to  find  which  of  Case  I  or  II  in  Proposition  4.1  holds,  we  keep  a  record  of  “areas”  of 

paths.  Let  t  be  any  directed  path  from  a:  point  P  to  Q.  Let  us  consider  a  cycle  consisting  of  three 
paths,  the  directed  line  segment  SP  (5  is  the  start  of  this  path  planning  problem),  7r  itself,  and 
another  directed  line  segment  QS.  An  area  A( ir)  of  tt  is  an  area  composed  by  this  cycle.  A{t)  is 
positive  if  the  cycle  is  counterclockwise  and  negative  otherwise.  Since  the  algorithm  for.  evaluating 
A(tt)  is  relatively  simple,  we  will’ not  give  the  details, here.  Using  this  area, 

Proposition  4.3  In  the  situation  described  in  Proposition  fl, 

•  Assume  the  mode  is ■+.  If 

—  A{tt\)  ~  A(Ai2’)  +  ^2).  >  0-  (13) 

the  closed  path  falls  in  Case  1.  otherwise  Case  II. 

•  Assume  the  mode  is  —.  If 

+  (14) 

the  closed  path  falls  in  Case  I,  otherwise  Case  II. 

4.4  Dijkstra’s  Algorithm 

Using  the  preparation  so  far,  applying  Dijkstra’s  algorithm  to  the  shortest  path  planning  problem 
in  a  circle  world  is  possible.  One  of  the  significant  changes  from  ordinary  Dijkstra’s  algorithm  is 
that  the  distance  from- 5  at  a  directed  circle  is  related  to  the  landing  point  to  it. 

Step  1.  Unmark  all  the  directed  (means  “with  mode”)  circles  and  let  their  distance  =  00.  Let 
dtotai  be  00. 

Step  2.  Assign  the  distance  of  all  visible  directed  circles  from  S  by  their  distances  from  S. 

Step  3.  Select  an  unmarked  directed  circle  Cm  which  contains  the  smallest  distance.  Mark  it. 
Step  4.  Update  the  distance  to  directed  circles  which  are  visible  from  Cm.  If  a  circle  already 
contains  a  finite  distance  value,  use  the  comparison  algorithm  described  in  Section  4.3. 

Step  5.  If  there  is  a  marked  directed  circle  which  is  visible  to  G ,  update  dtotai. 


Step  6.  If  dtotai.  is  less  than  or  equal,  to  the  distances  of  all  the  unmarked -directed;  circles,  stop. 
Otherwise  go  to  Step  3.  ' 

The  result  of  applying  Dijkstra’s  algorithm  to  the  sample  world  is  shown  in  Figure.12. 


5  A*  Algorithm 


In  this  problem,  the  use  of  the  A*  algorithm  is  natural  a;nd' effective  :[13].  Consider -a  partial  path 
7r  whose  last  landing  point  is  Pon  a  directed  circle  Cm.  A  lower  estimate  of  the  length  for  the  rest 
of  the  path  tt  (or  a  heuristic  fof'7r)‘is . the  direct  distance  between  P-and  the  goal  G.  However,  there 
is  a  better  heuristic  function. 

First,  take  a  tangent -from  Gm  to  G.  Let  Q  be,  the  leaving  point-on  Cm.  The  heuristic  function 
is  the  sum  of  a  partial  directed  circle  PQ  and  the,-Euclidean  distance  Q(?  (Fig.  13).  In  finding  the 
tangent  from  Cm  to  G,  their  is  no  need  to  calculate  visibility.  Even  if  the  tangent  from  Cm  to  G 
is  not  visible,  this  is  .a'  “lower"  estimate  and  a  valid  heuristic.  The  result  of  applying,  this  heuristic 
function, is;  depicted  in  Figure  14. 
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Figure  1:  Circle  World 


Figure  2:  Tangents  from  Point  to  Circle 


Figure  5:  Cross  and  External  Tangents 


Point-to-Point  Visiblity  Checks 


9  -  orientation  (pointl,  point2) 

91  -  orientation  (pointl,  drde.center)  *  9 

92  -  orientation  (point2,  drde.center)  -  9 

Figure  6:  Intersection  Test 


1S& 


Figure  7:  Tangents  from  Start 
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Figure  10:  All  Common  Tangents 


Figure  12:  Result:  Dijkstra’s  Algorithm 
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A*  Evaluation  Function  Comparison 


pathl  path2 


cost  (pathl)  +  arc_cost  (a,  b,  CW)  +  distance  (b,  goal)  > 
cost  (path2)  +  arc_cost  (c,  d,  CCW)  +  distance  (d,  goal) 

Figure  13:  Heuristic  Function 
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APPENDIX  E.  CIRCLE  WORLD  SOURCE  CODE 

/♦a********************  ******************  ******************* ********  **********  a  * 


Filename:  circle; c 

Purpose:-  Define-  structures,,  .type  definitions  and  functions  for 
circle_world"  robotics  pro  ject; 

Reference:-  Advanced  Robotics  class  notes;,  Dr.  YutakaKanayama 
Author:  Don. Brutzman 

Date:  10  February  .92 

Comments:  circle_world  is  a  set  of,  routines  for  mobile  robot  modeling- 

and  two-dimensional  path  planning., 

All  obstacles  are'  modeled  as  .circles'; 

Circles  -are  allowed  to  be-  adjacent  but  not  overlapping. 
Adjacent, (touching)  circles  do  not  prevent  robot  travel 
along,  either  circle  circumference. 

Language:  ANSI  C 

Compile:  cc  -g  -c  circle. c  -lm 

Graphing:  graph  -b  *?g,  1  rl  "circlev_world  plot"  <  filename-  I  lpr 

Status:  Shortest  path  solution  complete. 


* 

-* 

* 

* 

★ 

* 

* 

* 

* 

* 

* 

* 


* 

-g  *• 
*- 


***************************************** **************** **********************/ 


((include  <stdio.h> 
((include  <math.h> 

((include  <ctype.h> 

# define  CIRCLE. CONCLUDED 


/******  circle  world  Global  Constants  *****************************************/ 


((define  CCW  1 
((define  PLUS  1 
#define  RIGHT  1 
#define  POSITIVE  d 

((define  -CW  -i 
((define  MINUS  -1 
((define  LEFT  -1 
((define  NEGATIVE  -1 

((define  COLINEAR  0 
((define  CENTER  0 
((define  ZERO  0 


# define  TRUE 
#define  SUCCESS 
((define  YES 
# define  ON 

ffdefi:.ie  FALSE 
#define  FAILURE 
((define  NO 
Sdefine  OFF 

Idefine  VISIBLE 
#define  NONVISIBLE 
((define  TANGENTIAL 

fdefine  FATAL 
((define  NONFATAL 
#define  UNDEFINED 

((define  PI 


1 

1 

1 

1 

0 

0 

0 

0 

1 

0 

-1 

1 

0 

-1 

3.141592653589793 
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#define  TANGENTS_OK  TRUE  •/*  whether  or  not  tangents:  which,  pass  adjacent*/ 

/*  to  other  circles  are  considered.  VISIBLE  */ 

♦define  EPSILON  1.0E-6  /*  error -bound  in  floating- point  calculations  */ 

♦define  ARC_FACTOR  1.00  /*  factor  that  arc  lays  outside  circle,  plot  */ 

♦define  TICK_WIDTH  0.50  /*  tick  width  at  segment  endpoints  along -path  */• 


♦define  GRAPH_STRETCH  1.20  /*  expansion  factor  to  open:  out  graph  window  */ 

♦define  SUBDIVISIONS  360  /*  number  of  points  used  to  graphically  */' 

/*  represent  a-  circle  during  file  output  */ 

♦define  DEFAULT_Z  0.0  /*  Default  pool*  depth,  legal  range  0..8  feet  */ 

/*  where  zero  feet  =  surface  */ 

/*  makes  circle. auv  output  data  compatible  */ 
/*  with  NPS_Pool_Preview  graphics  project.  */ 


♦define  GRAPH_FILENAME  "circle. graph"  /*  name  of  graph  points  output  file  */ 
♦define- AUV_FILENAME  "circle. auv"-  /*  name  of  high  levei’  output  file  */ 
♦  define  TiU\CE  FALSE  /*  Enable  trace  printf  statements  in  circle. c  */ 


static  float  pooltime  =  0.0;-  /*  used  to  put  time  hacks  on  output  objects  */ 


/***  List  of  circle  world  Data  Structures  *************************************/ 
/* 

External  Data  Structures  Data  Types  and  Member  Labels 


Point 

double 

x,  y; 

Segment 

Point 

polntl,  point2; 

Circle 

Point 

center; 

double 

radius; 

Tangent 

Circle 

double 

circle; 

angle; 

Arc 

Circle 

double 

int 

circle; 

anglel,  angle2; 
rotation; 

♦Configuration 

Tangent 

double 

tangent; 

orientation; 

♦Path 

char 

Segment 

int 

Path_list 

♦label; 

initial_segment 

degree;- 

*path_list; 

*Circle_world 

Point 

start,  goal; 

int 

:Circle_list 

degree; 

*circle_l'ist; 

Internal  Data  Structures 

*Path_list 

Arc 

Segment 

Path_list 

arc; 

segment; 

♦previous,  *next 

*Circle_list 

Circle 

circle; 

.Circle_list 

♦previous,  *next 

V 


f v  & *  List  of  circir  world  Functions  ******************************************/ 
Functions  Parameters 
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(message,  fatal) 

(x,  y).  ’ 

(pointl,  point2) 

(point,  radius); 

(circle,  angle). 

(circle,  anglel,  angle2,  rotation); 
(tangent,  angle,  configuration) 
(initial_segment) 

(start,  goal,  circle_world) 

(x) 

(angle) 

(angle) 

(angle). 

(anglel,  angle2) 

(pointl,  point2) 

(pointl,  point2) 

•(pointl,  point2,  point3) 

(pointl,  ;point2,  point3) 
(poinfl->..point2,  point3) 

(pointl,  point2,  point3) 

(pointl,  point2,  point3) 

(circle,  angle) 

(segment,  circle) 

(pointl-,  point2,  circle_world) 


error 

make jpoint 

make_segment 

makejcircle 

make_tangent 

make_arc 

create_configuration 

♦createjiath 

create_circle_world 

sign 

degrees 

normalize 

normalize2 

precede 

orientation' 

distance 

angle 

S 

area 

order 

between 

circumference_point 

intersect 

visible 

circle_tangent 

arc_cost 

segment_cost 

augment_path 
add  circle_to_world 
fin3  circle 
retrTeve_circle_world 

graph_path 

graph~woirld 

output_path 

output“world 

center”graph_window 


(circlel,  circle2;  model, 
(arc)' 

(segment) 

(arc,  segment,  path) 
(circle,  circle-.world) 

(n,  circle_worl3)* 
(circle_world) 


mode2,  *configl,  *config2) 


yminptr,  ymaxptr. 


(path,  circle_wofld,  filename) 
(circle  world”  filename) 

(path,,  Tilename) 

(circle^world,  filename) 
(filename,  xminptr,  xmaxptr, 
magnification) 


See  c_search.c  (circle_search)  for  additional  data  .structures  and  functions. 


*/ 


/******  Circle_world  Data  Structures  and  Type  Definitions  »********************/ 

V* - - - - - - - ^ - 

typedef  struct  Point  type 

( 

double  x,  y;  /*  cartesian  coordinate  system  on  a  2D  plane  */ 

) 

Point; 


/* - ^ - */ 

typedef  struct  Segment  type 

( 

Point  pointl,  point2;  /*  Note  that  a  Segment  is  NOT  just  a  */ 

)  /*  collection  of  (double)  x,  y  coordinate  */ 

Segment;  /*  pairs,  i.e.  a  segment  is  a  pair  of  Points*/ 

/* - - - - - */ 


typedef  struct  Circle  type 

( 

Point  center; 

double  radius;  /*  radius  zero  circles  are  considered  points  */ 

) 

Circle; 


/* - - - -*/ 

typedef  struct  Tangent_type  /*  all  circle_world  angles  are  in  RADIANS  */ 
t 
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} 


Circle  circle; 

double  angle;  /*  angle  points  to  the -circle  tangent  point  */ 

/*  relative  to  the  circle  center  */ 

Tangent; 

/* - - - - - - - — — — - - - - - -— - - - — — — — - -----—*/ 

typedef  struct  Arc  type 

{ 

Circle  circle; 

double  anglel,  /*  anglel  points  to  landing  point  on-  circle  */ 

angle2;  /*  angle2  points,  to  leaving  point  on  circle  */ 


int  rotation;  /*  direction;  of  rotation  is  . either CW  -orCCW:  */ 

}  /*  RIGHT (+)  landing  tangent  =>  CCW(+)  rotation*/ 

Arc;  /*  LEFT  (-)  landing'  tangent  =>  CW  (-)  rotation*/ 

/*__ - - - -r - - - - - - - — - - */ 


typedef  struct  Configuration  type 

{ 

Tangent  tangent; 

double  orientation;  /*  orientation  .is  the  tangential  orientation,  */ 

}  /*  i.e.  the  direction  that  robot  is  pointing  */ 

Configuration; 

/* - - - - - - - - - - - - - - */ 

typedef  struct  Path  list  type 

{  " 

Arc  arc;  /*  arc  is  from  landing  point  to  leaving  point  */ 

Segment  segment;  /*  segment  connects  leaving, point,  either  to  */ 

/*  next  circle,  landing  point,  or  to  goal  */ 
struct  Path_li?t_type  /*  continue  this  type  with  doubly-linked  list  */ 

♦previous,-  /*  of  arc/segment  combinations  until  the  */ 

♦next;  /*  goal  is  reached  */ 

} 

Path_list; 


typedef  struct  Path_type 


char  *label;  /*  optional  string  label  for  each  path  */ 

Segment  initial_segment;  /*  initial  segment  leaving  start  point  */ 

int  degree;  /*  number  of  arc/segment  pairs  in  the  path  */ 

Path_list  *path_list;  /*  linked  list  of  arc/segment  combinations  */ 

Path; 

/* - - - - - - */ 


typedef  struct  Circle  list  type 
{  "  " 

Circle  circle; 

struct  Circle_list_type  /*  continue  this  type  with  doubly-linked  */ 

♦previous,  ~  /*  list  of  remaining  circles  */ 

♦next; 


} 

Circle_list; 

/*- - - - - */ 

typedef  struct  Circle_world_type 

Point  start;  /*  starting  point  */ 

Point  goal;  /*  goal  point  */ 

int  degree;  /*  number  of  circles  in  this  world  */ 

Circle_list  *circle_list;  /*  linked  list  of  circles  in  world  */ 

Circle  world; 

/* - - - - - - - */ 


/*♦***♦  circle  world  Function  Declarations  ************************************/ 
/* - - - */ 


void  error  (message,  fatal)  /*  internal  error-handling  diagnostic  */ 

char  ‘message;  /*  error  message  to  be  printed  */ 
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int  fatal; 


/*  exit  if  FATAL,  return- otherwise.  */ 

{ 

fprintf  (stderr,.  "***  Program  error:  ")•; 
perror  (message) ; 
fprintf  (stderr,  "\n\n") ; 
if  (fatal  ==  FATAL) 

( 

fprintf  (stderr,  “FATAL  error,  program  exit.\n") ; 
exit  (FAILURE); 

} 

return; 

) 

/* - - - — — - - - — - - - -*/ 

Point  make_point  (x,  y)  /*  make  a  point  from  x,  y  coordinates  */ 

double  x,  y; 

< 

Point  point;. 

point. x  =  x; 
point. y  =  y; 


return  point; 

) 

/* - r--~ 


Segment  make_segment  (pointl,  point2) 
Point  pointl,.  point2; 


{ 

Segment  segment; 


- - - - - --*/ 

/*  make  a  Segment  from  two  points  */ 


segment. pointl  “pointl; 
segment. point2  =  point2; 

return  segment; 

) 

/*--- - - - - - - - - - - */ 

Circle  make_circle  (point,  radius)  /*  create  a  circle  from  point  &  radius  */ 

Point  point; 
double  radius; 

( 

Circle  circle; 


circle. center  *>  point; 
circle. radius  «  radius; 


return  circle; 

) 

/* - */ 

Tangent  make_tangent  (circle,  angle)  /*  make  tangent  from  circle  s  angle  */ 

Circle  circle; 
double  angle; 

{ 

Tangent  tangent; 

tangent. circle  =  circle; 
tangent. angle  =  angle; 

return  tangent; 

) 


/*-■ 

- */ 

Arc 

make_arc 

(circle,  anglel. 

angle2. 

rotation) 

Circle 

circle; 

/* 

create  an  arc  type  from  circle 

data,  */ 

double 

anglel,  angle2; 

/* 

two  angles  anH  a  direction  of 

rotation*/ 

{ 

int 

rotation; 

/* 

(CW  or  CCW) 

*/ 

Arc  arc; 

/*  initialize  values  */ 

arc. circle 

=  circle; 

arc.anglel 

=  anglel; 

arc.angle2 

»  angle2; 

arc. rotation  =  rotation; 
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return  arc; 

} 

/*-- - - — - - r - — - - - — - - - ---*/ 


void  create_configuration  (tangent,  orientation,  configuration)’ 


Tangent  tangent; 

double  orientation; 

Configuration  ‘configuration; 


/*  create  a  conf iguration^type  pointer  */ 
/*  from  a  tangent  and. an- orientation  */ 
/*  resulting  configuration  output  */ 


if  (configuration  ==-NULL)-  /*  allocate,  memory  if  needed  */ 

{  -  ' 

if  (TRACE)  printf  ("\n“*  create  configuration:  allocating  memory"); 
if  ( (configuration  =  (ConfiguratTon  *)  malloc  (sizeof  (Configuration) ) ) 
==  NULL) 

error  ("create  configuration:  memory  allocation  failure",  FATAL); 

> 


/■*  initialize  values  */ 


configuration->tangent  =  tangent; 
configuration->orientation  =  orientation; 

return; 

) 

/* - - - - - - */ 


static  Path  *create_path  (initial_segment) 


Segment  initial_segment; 


( 

static  Path  ‘path; 


/*  begin  a  path  type  linked  list,  using  */ 
/*  a  segment  which  includes  start  point  */ 


if  ( (path  ”  (Path  *)  malloc  (sizeof  (Path) ) ).  ”  NULL) 

error  ("create_path:  memory  allocation  failure",  FATAL); 

/*  initialize  values  */ 


path->label  =  "  "; 

path->degree  *  0; 

path->initial  segment  =  initial  segment; 

path->path_list  =  ((Path_Tist  *)  0); 

return  path; 


/* - - - - - - - - - - - */ 

void  create^circle_worid  (start,  goal,  world) 

Point  start,  goal;  /*  create,  a  circle^world  using  •*/ 

/*  start  and  goal  points  only  '*/ 

Circle_world  ‘world;  /*  resulting  circle_world  output  */ 

if  (world  ==  NULL)’  /*  allocate  memory  if  needed  */ 


if  (TRACE)  printf  ("\n*“  create  circle_world:  allocating  memory") ; 
if  ((world  =  (Circle  world  *)  malloc  (sizeof  (Circle .world) ) )  ==  NULL) 
error  ("create  circle  world:  memory  allocation  lailure",  FATAL); 

}  " 

/*  initialize  values  */ 

world->start  =  start; 

world->goal  =  goal; 

world->degree  =  0; 

world->circle_list  =  ( (Circle_list  *)  0); 

return; 


/* - * / 

int  sign  (x)  /*  return  sign  of  x  as  an  integer  */ 


double  x; 

if  (x  >  0.0) 

return  POSITIVE; 
else  if  (x  <  0.0) 
return  NEGATIVE; 
else 

return  ZERO; 
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double  degrees  (angle)  /*  conversion  from  radians  to  degrees  */• 

double  angle;  /*  note  no  normalization  is  performed  */ 

( 

return  angle  *  180.0  /  PI; 

} 

/* . . . . . . . . - - - 

double  normalize  (angle)  /*  standard  normalization  range  -PI.. PL  */ 

double  angle;  /*  angle  is  any  real  value  in  RADIANS  */ 


{ 

double  x; 
x  »  angle; 

while  (x  >  PI)  x  -  x  -  PI  -  PI; 
while  (x  <  -  PI)  x  -  x  +  PI  +  PI; 
return  x; 


/* - */ 

double  normalize2  (angle)  /*  alternate  normalization  range  0.,2*PI  */ 

double  angle;  /*  angle  is  any  real  value  in  RADIANS  */ 


< 

double  x; 
x  »  angle; 

while  (x  >  PI  +  PI)  x  -  x  -  PI  -  PI; 
while  (x  <  0.0)  x  ■  x  +  PI  +  PI; 
return  x; 

) 

/* . . . . . */ 

int  precede  (anglel,  angle2)  /*  Boolean  function  for  angle  precedence  */ 

double  anglel,  angle2;  /*  angles  are  any  real  angles  in  degrees  */ 

/*  return  TRUE  if  anglel  precedes  angle2,*/ 
/*  return  FALSE  otherwise  */ 

{ 

/*  note  that  the  input  angles  are  individually  normalized  to  ensure  validity  */ 

if  (normalize  (normalize  (angle2)  -  normalize  (anglel))  >  0.0) 
return  TRUE; 

else  /*  reference;  equation  (7)  class  notes  */ 


return  FALSE; 

} 

/* . . . V 

double  orientation  (pointl,  point2)  /*  range  -PI  ..PI  */ 


Point  pointl,  point2;  /*  return  normalized  angle  between  points*/ 

if  ( (pointl. x  =“  point2.x)  tt  (pointl. y  =«  point2.y)) 

return  0.0; 

else 

return  normalize  (atan2  (point2.y  -  pointl. y,  point2.x  -  pointl. x)); 


double  distance  (pointl,  point2)  /*  euclidean  distance  between  two  points  */ 
Point  pointl,  point2; 

{ 

double  deltax  =  point2.x  -  pointl. x; 
double  deltay  =  point2.y  -  pointl. y; 
return  sqrt  (deltax  *  deltax  +  deltay  *  deltay); 


/* - */ 

int  angle  (pointl,  point2,  point3)  /*  return  angle  between  three  points  */ 


/*  angle  order  is  pointl. .point2. .point3  */ 

Point  pointl,  point2,  point3; 


{ 

return  normalize  (orientation  (point2,  pointl)  - 
orientation  (point2,  point3));- 

) 

/* - */ 

double  S  (pointl,  point2,  point3)  /*  calculate  S  function  for  three  points  */ 

/*  reference:  equation  (14)  class  notes  */ 

Point  pointl,  point2,  point3; 
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/*  CCW-tripiesare  positive  s  CW  triples  are  negative,  matching  conventions.  */ 

( 

return  O.S  *  ((point 2.x  -  pointl.x)  *  (point3.y  -  pointl.y)  - 
(point3.x  -  pointl.x)  *  (poiht2.y  -  pointl.y)’); 

)  ’  '  ' 

/* - - - j - - - - - - - r - - -- - - - --*/ 

double  area  (pointl,  point2,  point!)  /*  calculate  area  between  three  points  *7 


/*  -reference:  Proposition  3.1  class  notes*/ 

Point  pointl,  point2,  point3; 

< 

return  fabs  (S  (pointl,  point2,  point3)); 

) 

/*— - - - - - - - - V 


int  order  (pointl,  point2,  polnt3)/*  determine  order  of  three  points  */ 

Point  pointl,  point2,  point3;/*  relationship  between  three  points  is  */ 

/*  clockwise  (CW) ,  counterclockwise  (CCW)  */■ 
/*  or  COLINEAR  */ 

/*  reference:  equation  (15)  class  notes  */ 

{ 

if  (fabs  (S  (pointl,  point2,  point3))  <»  EPSILON) 

return  COLINEAR;  /*  floating  point  error  check,  correction*/ 


else 

return  sign  (S  (pointl,  point2,  point3)); 

) 

/* . */ 

int  between  (pointl,  point2,  point3)  /*  test  for  betweenness  of  point2  */ 


Point  pointl,  point2,  point3; 

if  ((order  (pointl,  point2,  point3)  —COLINEAR) 

(pointl.x  <*■  point2.x)  it  (point2.x  <■  points. x)  tt 
(pointl.y  <■  point2.y)  ti  (point2.y  <»  point3.y)) 
return  TRUE; 

else 

return  FALSE; 

) 

/* . */ 

Point  circumferencejpoint  (circle,  angle)  /*  return  coordinates  of  a  point  */ 

/*  on  a  circle's  circumference  */ 

^  Circle  circle;  double  angle; 

return  make^point  (circle. center. x  +  circle. radius  *  cos  (angle), 
circle. center. y  +  circle. radius  *  sin  (angle)); 


int  intersect  (segment,  circle)  /*  determine  if  segment  intersects  circle  */ 

Segment  segment; 

Circle  circle; 


{ 


/ft*************************************************************************/ 


/*  '.eturn:  TRUE  if  any  intersection  exists  within  the  circle  */ 

/*  return:  FALSE  if  no  intersections  exist  within  the  circle  */ 

/*  return:  TANGENTIAL  if  only  intersection  is  within  EPSILON  of  the  */ 

/*  circle  circumference  (i.e.  tangential),  */ 

/*  TANGENTS_OK  is  defined  as  TRUE,  t  neither  */ 

/*  segment  endpoint  is  on  the  circumference.  */ 

/********************************************************»*****************/ 

/*  local  variable  declarations  */ 

double  height,  /*  height  of  circle  center  from  segment  */ 

theta,  /*  orientation  angle  of  segment  */ 

difference;  /*  difference  between  circle. radius  and  */ 

/*  distance  of  circle. center  to  segment  */ 


/*  check  special  case:  circle  center  is  on  the  line  segment  */ 

if  (between  (segment. pointl,  circle. center,  segment. point2) ) 

if  (TRACE)  (printf  ("\n***  intersect:  betweenness  case  ”); 
printf  ("  (%3.1f,  %3.1f>  (%3.1f,  %3.1f)", 
segment. pointl.x,  segment. pointl.y, 
segment. point2.x,  segment. point2.y) ; ) 
if  (circle. radius  <=  EPSILON)  /*  this  accounts  for  "point"  circles  */ 
return  TANGENTIAL; 
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else 

return  TRUE;  /*  intersection  occurred’.  */ 

> 

/*  determine  theta- ■  orientation  angle  of  segment  */ 

theta  =  orientation  (segment. pointl,  segment. point2) ; 

if  (TRACE)- -printf  ("\n***  intersect:  "); 

/*  check  case  where  circle; center  is  on  right -hand  side  of  segment  *./ 
if  (fabs  (normalize  (orientation  (segment. point2,  circle. center)  -  theta)') 
<-  (PI/2.0) ) 

{ 

difference  -  distance  (segment. point2,  circle. center)  -  circle. radius; 
if  (TRACE)  printf.  ("case  1  ")  ; 

if  (TRACE)  printf  ("orientation  =  %f  ", 

degrees  (orientation  (segment. point2,  circle. center) )) ; 
if  (TRACE)  printf  ("  theta  «■  If  ",  theta); 
if  (TRACE)  printf  ("  normalize  «  %f 

degrees  (normalize  (orientation  (segment, point2,  circle, center) ))) ; 

) 

else  /*  check  case  where  circle. center  is  on  left  hand  side  of  segment  */ 
if  (fabs  (normalize  (orientation  (segment. pointl,  circle. center)  -  theta)) 
>-  (PI/2.0)) 

( 

difference  ■  distance  (segment. pointl,  circle. center)  -  circle. radius; 
if  (TRACE)  printf  ("case  2  "); 

) 

else  /*  check  case  where  segment  length  is  zero  */ 

if  (distance  (segment. pointl,  segment. point2)  »«  0.0) 

( 

difference  -  distance  .(segment .poiptl,  -circle. center)  -  circle. radius; 
if  (TRACE)  printf  ("case  3  "); 

) 

else  /*  circle. center  is  in  a  voronoi  region  defined  by  segment  edge  */ 

height  »  area  (segment, pointl,  segment. point2,  circle. center) 

*  2  /  distance  (segment. pointl,  segment. point2) ; 
difference  -  height  -  circle. radius; 
if  (TRACE)  printf  ("case  4  ; 


/*  Now  use  'difference'  to  test  for  tangency  or  intersection.  */ 

if  (TRACE)  (printf  ("difference  -  4f  ",  difference); 

printf  ("  (%3 .If,  %3 .If)  (*3.1f,  %3.1f)", 

segment .pointl .x,  segment .pointl .y, 
segment. point2,x,  segment. point2.y) ; 
printf  ("  circle  (43. If,  *3. If)  ", 

circle. center, x,  circle. center. y) ; 

) 

if  (difference  <  -  EPSILON)  /*  circle. radius  is  greater  than  the  */ 

return  TRUE;  /*  circle's  distance  from  the  segment  */ 

/*  thus  intersection  is  TRUE  */ 

else  if  ((fabs  (distance  (segment. pointl,  circle, center)  -  circle. radius) 

<-  EPSILON)  I  I 

(fabs  (distance  (segment. point2,  circle. center)  -  circle. radius) 

<=  EPSILON)) 

return  FALSE;  /*  ignore  external  tangency  of  segment  endpoints,  */ 
/*  because  solely  trying  to  determine  tangency  */ 

/*  with  different  circles  in  circle_world  */ 

else  if  ((fabs  (difference)  <=  EPSILON)  it  TANGENTS  OK  ««  TRUE) 
return  TANGENTIAL; 

else 


return  FALSE;  /*  circle  does  not  intersect  segment  */ 

) 

/* - */ 


int  visible  (pointl,  point2,  circle_world) 

Point  pointl,  point2;  /*  determine  whether  a  direct  path  is  */ 

Circle_world  *circle_world;  /*  visible  between  two  points  without  */ 

/*  crossing  any  circles  in  circle_world*/ 

/******************** ****************************************************** / 
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/*  return:  -VISIBLE  if  no  intersections  exist  with  circle_;world  */ 

/*  return?  NONVISIBLE  if  any  intersection  exists  with  circleTworld:  */ 

/*  return:  TANGENTIAL  if  ;only  intersection  (s)  are  , within  EPSILON  of'  */- 

/*  circle  circumference  (s):-,  i’si.  "tangential-,  */ 

^★★★★★★★♦★★★★★★★★★★★★★★****************** ★★*★★*★* ★★★*****★*★*★*★**★*★★ **★**/ 

'/*  local  variable  declarations  ,  -*/ 

int  i,  -  /*  in^ex  */ 

visibility;  •/*  VISIBLE, ,  NONVISIBLE-  of  .TANGENTIAL  */ 

Segment  segment;  /*  intersection’  of  perpendicular  S  line-  */ 

Circle  circle;  /*  local  variable  holdihg-.current  circle  */ 

Circle_list  *world_ptr;  /*  pointer  to  current  circle  */ 

visibility  »  VISIBLE;  /*  default  initialization  */- 

segment  -  make  segment  (pointl,  point2); 

world_ptr  ■  circTe_world->circle_list;  /*  first  circle  in  world'  */ 

/*  Note  that  intersection  with  circle_world  start  &  goal  is  not.  checked.  */ 

for  (i»l;  i  <»  circle  world-^degree;  ++ij' 

{  ", 

/*  check  next  circle  in  clrcle_world'  for  intersections  */ 

circle  ■  world_ptr->circle;  — 

if  (intersect  .(segment,  circle).*”  TANGENTIAL) 

visibility  «  TANGENTIAL;  /*  i.e.  close  enough  to  be  a  tangent*/ 

/*  continue- searching  */ 

else  if  (intersect  (segment,  circle)  ■«  TRUE) 

< 

if  (TRACE)  (printf  ("\n***  visible  complete:  NONVISIBLE\n") ; ) 
visibility  -  NONVISIBLE; 
return  visibility; 

) 

world  ptr  »  world  ptr->next; 

}  "  _  ' 

if  ((Visibility  —  VISIBLE)  £C  TRACE) 

printf  ("\n***  visible  complete:  VISIBLEVn”) ; 

else  if  ((visibility  =”  TANGENTIAL)  it  TRACE) 

printf  ("\n***  visible  complete:  TANGENTIAL\n") ; 

return  visibility; 


}. 

/* . - — */ 

void  circle_tangent  (circlel,  circle2,  model,  mode2,  configl,  config2) 

Circle  circlel,  /*  input:  Leaving  circle  where  tangent  starts  */ 

circle2;  /*  input:  Landing  circle  where  tangent  ends  */ 

int  model,  /*  input:  which  side  of  circlel  */ 

mode2;  /*  input:  which  side  of  circle2  */ 

Configuration  *configl,  /*  output:  starting  configuration  pointer  */ 


*config2;  /*  output:  ending  cpnfiguration  pointer  */ 

double  alpha,  theta,  delta,  anglel,  angle2;  /*  local  declarations  */ 
Circle  circle3;  /*  input:  Leaving  circle  where  tangent  starts  */ 
Circle  circle4;  /*  input:  Leaving  circle  where  tangent  starts  */ 


circle3  «  circlel; 
circle4  «  circle2; 

theta  =  orientation  (circlel. center,  circle2. center) ; 

/*  Simplified  delta  angle  equation  originated  by  LT  Scott  Starsman  USN  */ 

delta  =  asin  ( (mode2  *  circle2. radius  -  model  *  circlel. radius)  / 
distance  (circle2. center,  circlel. center) ) ; 

alpha  »  normalize  (theta  -  delta);  /*  tangential  orientation  */ 

anglel  “  normalize  (alpha  -  model  *  PI  /  2);  /*  leaving  angle  circlel  */ 

angle2  “  normalize  (alpha  -  mode2  *  PI  /  2) ;  /*  landing  angle  circle2  */ 


if  (model  ==  CENTER) 

{ 

anglel  =  0.0; 

circle3. radius  =  0.0; 

) 

if  (mode2  ==  CENTER) 

( 

angle2  =  0.0; 

circle4. radius  =  0.0; 
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'if  (TRACE)-  .  ...... 

printf  -(n\h***  cirole_Cangents :  theta  =  %f,.  delta  =  %f,  alphas  %f ,  Yn", 
degrees  (theta) v  degrees  (delta) ,  degrees  (alpha)  );, 
if  (TRACE)  printf  (?  'ahglel  =  %f,  ari'gle2'  =  %f\n", 

degrees  i(anglel) ,  degrees  (angle2)); 

configl->tangent  =  make^tarigent  (circle3,  ahglel); 

configl->orientation  =  alpha;  '  . 

config2->tangent  *■  make\tangent  (circle4,  angle2)'; 

cohfig2r>orientation  *■  alpha; 

return;' 


double  arc  cost  .(arc) 


/*  euclidean  distance  cost  function  */ 

/*  rotation  direction  is' included  in  arc  */. 


Arc  arc;  /*  rotation  direction  is' includec 

double  delta_angle; 
if  (arc. rotation  CW) 

delta_angle  “  normalize  (arc.anglel)  -  normalize  (arc. angle2) ; 
else  if  (arc. rotation  --  CCW) 

delta_angle  -  normalize  (arc.angle2)  -  normalize  (arc.anglel); 

else  if  (arc. rotation  ■■  ZERO) 
delta_angle  -  0.0; 


delta  angle  ■  normalize  (arc.anglel)  -  normalize  (arc.anglc2)-; 
prihtT  ("\nlllegal  rotation  value  (%ld)  given  to  arc_cost  function,", 
"  Assumed  CLOCKWISE. \n",  arc. rotation) ; 

) 

/*  circumference  portion  »  2  *  PI  *•  R  *  (delta_angle  /  (2  *  PI)) 
return  arc. circle. radius  *  normalize2  (delta_angle) ; 


double  segment_cost  (segment) 
Segment  segment; 


/*  euclidean  distance  cost  function 


return  distance  (segment. pointl,  segment. point2) ; 


void  augment_path  (arc,  segment,  path) 


Arc 

Segment 

Path 

Path_list 

Path-list 


arc; 

segment; 

*path; 

*path_ptr; 

*path”node; 


/*  add  arc  and  segment  to  path  */ 
/*  the  order  of  adding  an  arc  followed  */ 
/*  by  a  segment  is  a  rigorous  requirement*/ 

/*  index  pointer  to  legs  on  the  path  V 
/*  local  variable  to  build  path  leg  */ 


if  ( (path_node  =  (Path_list  *)  malloc  (sizeof  (Path_list) ) )  ==  NULL) 
error” ("augment  jpath:  memory  allocation  failure!",  FATAL); 

/*  initialize  values  of  path_node  which  will  augment  current  path 

path_node->arc  «  arc; 

path~node->segment  =  segment; 

pa£h_node->next  *>  ((Path_list  *)  0); 

path”node->previous  =  ((path_list  *)  0); 


if  (path->degree  ==  0) 

path->path_list  =  path_node; 


/*  first  path  in  path_list  to  be  added 


/*  point  to  first  leg  of  path,  then  find  end  of  current  path_list 
path_ptr  =  path->path_list; 
while  (path_ptr->next  !=  {(Path_list  *)  0)) 
path_ptr  =  path_ptr->next; 

/*  now  augment  current  path  with  new  path  leg 
path_node->previous  =  path_ptr; 
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path_ptr  ->next  -  path_node;. 

pathf->degree.++;; 
if  (TRACE)*  . 

*  .printf  ("\n***  path->degree  =  %i,,  augmentvpath;  complete Vp?;.  path->degree) ; 
return;, 

} 

T - -■ - - 

void  add_circleJ:o_world.  (circle,  circle_world) 

Circle.  circle;  /*  circle  tobe  added'to  world  */ 

Circle*  world  *circle  world;  /*  current  circle*  world'  ■*/ 

{  '  ,  " 

Circle_list  *'circlejptr;  /*' index  .pointer ‘to  current  circle  */ 

Circle_list  *circle_node;  /*  local  .variable  to -build, circle  leg  */ 

double  separation;  /*  usedto  check  S  prevent  circle  overlap*/ 

if  (TRACE)  ,printf  ("\n***  add_circle_toj_world  start\n"); 

if  ( (circle^_node  -  (Circle_list  *)  malloc -(sizeof  (Circle_list) ) )  —  HULL) 
error  (T7add_circle_to_world:  memory  allocation  failure! FATAL); 

separation  *=  distance  (circle. center,  circle_world->star,t) ," 
if  ((separation  -  circle. radius)  <  -  EPSILON) ’ 

( 

printf  ("\n***  add^circle_to._world:  the  new  circle  at  ("); 
printf (!!%4.2f>  %472f)  \n";  circle,  center,  x*, 

circle. center .y) ; 

printf ("is  not  being  added-because  it  overlaps  the  start  point"); 
printf("  at  (%4.2f,  %4.2f)  \n\n",  circle_world->start.x, 

circle~world->start.y) ; 

return; 

-) 

separation  **  distance  (circle. center,  circle^world->goal) ; 
if  {(separation  -  circle. radius)  <  -  EPSILONj 

printf ("\n***  add_circle_to_world:  the  new  circle  at  ("); 
printf ("%4.2f,  %4T2f)  \n",  circle. center. x, 

circle. center. y) ; 

printf ("is  not  being  added  because  it  overlaps  the  goal  point"); 
printf("  at  (%4.2f,  % 4 . 2 f )  \n\n",  circle_world->goal.x, 

circle“world->goal.y) ; 

return; 

) 

/*  initialize  values  of  circlejaode  which  will,  be  added  to  circle_world  */ 
circle_node->circle  -  circle; 

circle~node->next  -  ( (Circle_list  *)  0); 

circle~node->previous  -  ( (Circle“list  *)  -0); 

if  (circle_world->degree  »=  0)  /*  first  circle  in  circle  world  to  be  added*/ 
circle“world->circle_list  »  circle_node; 

else 

{ 

/*  point  to  first  circle  in  world,  then  find  end  of  current  circles  */ 
circle_ptr  =  clrcle_world->circle_list; 

while  (circle_ptr->next  !  =  ( (Circle_list  *)  0)) 

separation  =  distance  (circle. center,  circle_ptr->circle. center); 
if  ((separation  -  circle. radius  -  circlejptr->circle. radius) 

<  -  EPSILON)' 

{ 

printf ("\n***  add_clrcle_to_world:  the  new  circle  at  ("); 
printf  ("t4.2f,  %472f)  \n“’,  circle. center. x, 

circle. center. y) ; 

printf ("is  not  being  added  because  it  overlaps  the  circle"); 
printf ("  at  (%4.2f,  %4.2f)  \n\n",  circle_ptr->circle. center. x, 

circle_ptr->circle. center. y) ; 

return; 

) 

circle  -ptr  **  circle _ptr->next; 

) 

/*  now  check  that  pesky  last  circle  */ 

separation  °  distance  (circle. center,  circle__ptr->circle. center)  ; 
if  ((separation  -  circlelradius  -  circle_ptr->circle. radius) 

<  -  EPSILON) 

{ 

printf ("\n***  add_circle_to_world:  the  new  circle  at  ("); 
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printf ("%4.2f,  %4.2f)  \n",  circle ; center. x, 

circle. center. y) 

printf  ("is  not  being,  added  because  it  overlaps  .the.  circle") ; 
printf  ("  at  (%4~.2f,  %4.2f)  \nKn",  cifcle_J>trT>circle:  center. x,. 

circlejptr-:>circle.center:y) ! 

return; 

)  '  . 

/*  now  add  new  circle^node  to  current  circle__list  in- circle -world  */ 
circle_node->previous  =  circle__ptr; 
circle_ptr  ->next  =  circle_node; 

circle_world-r>degree++; 

if  (TRACE)  printf  <"\n***  circle^world-Sdegree  -  %i\n", 

,circle_worldr>.degfee) ; 

if  (TRACE)  printf  (f?\n***  add_circle_to_world  complete\h") ; 
return; 

) 

/* - - - - - -- - - - - - - - - - : - - - r-- - - - - - - */ 

Circle  find^circle  (n,  circle_world) 

int  n;  /*  get  the  nth  circle  from  circle_world  *'/ 

-Circle- world  *circle_world; 

(  “ 

int  i,  nn; 

Circle  circleO; 

Circle_list  *circle_ptr; 
nn  =  nT 

if  (circle  world->degree  —  0) 

( 

printf  ("\n***  find_eircle:  there  are  no  circles  in  circle_world; ", 
"\n  using  the  start  point  as  a  zero-radius  circle. \n"); 
circleO  -=  make_circle-  (circle_worldT>start,  0.0) ; 
return  circleO; 

) 

while  ( (nn  <-  0)  I  I  (nn  >  circle  world->degree) ) 

(  . 

printf  ("\n***  find  circle:  there- are  %d,  circles  in  circle_world.*", 
circle  worTd->degree) ; 
printf  (','  WhicE  do  you  want?  "); 
scanf  ("%d",  inn);  printf  ("\n"); 

} 

/*  ready  to  go;  point  to  first  circle  in  world,  then  find  n  th  circle  */ 
i  *  1; 

circle  ptr  =  circle  world->circle_1;list; 
while  "((i  <  nn)  t&  Tcircle  ptr->next  !=  NULL) ) 

( 

circle_ptr  «  circle  ptr->next; 

++i; 

) 

return  circle  ptr->circle; 

) 

/* - - - */ 


void  graph_path  (path,  circle_world,  filename) 

/*  Print  path  data  for  unix  'graph'  use,  appended  to  'filename'  */ 


( 


Path  "path; 

Circle_world  *circle_world; 
char  ~  ‘filename; 


FILE 

double 

int 
Point 
Circle 
Path  list 


*file_ptr; 

begin  angle,  endjangle,  delta  angle,  angle, 
midpoTnt_x,  midpoint  y,  xl,  yl,  x2,  y2; 

i,  j,  n,  rotation;  7*  indices,  t  arc  steps  t  local  variable  */ 

point; 

arc  circle; 

*patE_ptr;  /*  index  pointer  to  legs  on  the  path  */ 


if  (TRACE)  printf  ("\n***  graph_path  start\n"); 

if  (path  *=»  NULL)  printf  ("\n***  path  ==  NULL,  error !\n"); 


path_ptr  =  path->path_list;  /*  point  to  first  leg  of  path  */ 

if  ((file_ptr  =  fopen  (filename,  "a"))  ==  ((FILE  *)  0)) 

error  ("graph_path:  file  open  failure!",  NONFATAL); 
return; 
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}. 

/*  print  starting  line  segment  .  , .  _ _  */ 

if  (TRACE)  printf  (."  %f  %f\n",  path->initial_segment.p'oiritl.x, 

. .  ,  path->initial_segment .pointl.y) ; 

•if  (TRACE)  printf  ("  %f  ,%f\n\"  \"\n",  .patK->init.ial_segment  .polnt2'.x, 

path->inicial_segment .point 2.y); 

fprintf  (filejptr,  "  %f' %f\n",  path->initial_segment. point!,  x, 

path-!.initial_segment  .pointl  .y)  ; 

fprintf  (filejptr,  "%f%f\n\"  \"\n",  path->initial_segment .point2 .  x, 

path->initial_segment .point2.y) ; 


/*  Print  tick .marks  perpendicular  to  endpoint  of  initial  segment 


if  ( ( (p‘ath->initial_segment.pbint2.x 
(path->initial_segment.pbint2.y 
((path->initial_segment.poiht2.x 
(path->initial~segment.point2.y 


.=  circle_world->start.x)  1 1 
=  circle_world->staft.y) )  SS 
=  circle- world->goal.x)  .  1 1 
=  circle_world->goal.y) ) j 


{ 

angle  °  orientation  ,(path->initialjsegment.pointl, 

path->initial3segment.p6int2)  +  -(PI/2.0); 
xl  =  path->initial_segment.point2.x  -  (TICK_WIDTH  /  2,0)  *  cos  (angle); 

yl  =  path->initial2s_egmeht.p'oint2.y,  -  (TICKJWIDTH  /  2.0)  *  sin  (angle); 

x2  =  path‘->initial-segment.pqint2.x  +  (TICK- WIDTH  2.0)  *  cos  (angle); 

y2  =  path->initial— segment. point2.y  +  (TICK_WIDTH- /  2.0)  *  sin  (angle); 

fprintf  (filejptr,-"  %f  %f\n  %f  %f\n\"  \",\n",  xl,  yl,  x2,  y2); 
if  .(TRACE)  printf  ("tickmark  at  end  of  initial  segment:  \n"); 
if  (TRACE)  printf  ("  %f  %f\n  $f  %f\n\"  \"\n".,<  xl,  yl;  x2,  y2); 

/*  Print  path  label  adjacent  ‘to  midpoint  of  initial_segment 
midpoint_x  =  (path->initial_segment-.poiritl.x  +  *  ~ 

path->lnitial2segment.point2.x)  /  2.0; 
midpoint_y  =  ,(path->initial-segment.pointl.y  + 

pathr>initial-segment.pdint2jy)  /  2.0; 
if  (TRACE)  printf'  ("  %f  %f\n!',  mldpoint_x,  midpoint_y)  ;■ 
fprintf  (file_ptr,  "  %f  %f\n'!,  midpoint~x,  midpoint_y); 


*/ 


if  (TRACE) 

{ 

if  (path->labe!  !»  NULL)  printf  ("\"%s  \"\n",  -path->label) ;. 
else  printf  ("\"  \"\n") ; 

) 

if  (path->label  !»  NULL)  fprintf  (fileptr,  "\"%s  \!,\n",  path->label) ; 

else  fprintf  (filejjtr;  "\"  \"Vn"); 


/*  print  all  succeeding  arc  /  line  segment  combinations  */ 

for  (i*»l;  i  <«  path->degree;  ++i,  path  ptr  ■  path'  ptr->next) 

(  .  ~ 

/*  Calculate  and  plot  arc  traversal  points  */ 


begin_angle  ■  normalize2  (path_ptr->arc.anglel) ; 
end_angle  =  normalize2  (pathjptr->arc.angle2) ; 
rotation  =*  path  jptr->arc.  rotation; 

if  (fabs  (begin_angle  r  end_angle)  <=  EPSILON) 

delta__angle~“  0.0;  ~ 

else  if  ((precede  (end_angle,  begin_angle)  ss  (rotation  =»  CCW)j  II 
(precede  (begin_angle,  end~angle)  ss  (rotation  »=  CW) ) ) 
delta_angle  «■  PI  +  PI  -  faEs  (end_angle  -  beginjingle)  ; 

else 

delta_angle  =  rotation.  *  (end_angle  -  beginjingle) ; 

-delta_angle  =  normalize2  (delta_angle) ; 
angle  =  begin_angle;  ~ 

arc_circle  »  path_ptr->arc. circle; 

n  =  (int) .( (float)  (SUBDIVISIONS)  *  fabs  (delta_angle)  /  (PI  +  PI)  +  .5); 
if  ( (delta_angle  »■»  0.0)  ||  (n  <*■  0)  ||  (rotation  =«  CENTER)) 

n  »  0;~  /*  perform  only  one  iteration  of  loop  */ 

else 

{ 

/*  Print  first  point  of  arc  without  ARC_FACTOR  for  continuity  */ 

point  »  circumference_point  (arc_circle,  begin_angle) , 
fprintf  (fileptr,  "  %f  %f\n",  point. x,  point. y); 
if  (TRACE)  prTntf  ("  %f  %f\n",  point. x,  point. y); 

) 

if  (TRACE) 

( 

printf  ("\n***  graph  path  n  =  %d,  delta_angle  =  %f\n", 
n,  degrees (3elta_angle) ) ; 
printf  (  "  begin_angle  =  %f,  ", 

degrees  (begin_angle7) ; 
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printf  (?end,  angle  =  %f,  rotation  =  %d\n\n",  , 
Hegrees  (end_angle) ;  rotation) ; 


/•  Factor  radius- to  graph  airc  just  outside  . Circle  circumference  */ 

arc_circle. radius  *=  ARC_FACIOR; 


/*  calculate  and  print  points  for  the  arc  -starting' from-  initial,  angle  */ 
for  -'(j  =  0;  j  <=  n;  ++j) 

{  ;  . 

if  (TRACE)  printf  ("***  graph_path:  -j  =  %d,  angle  =  %f,  n  =  %d  \n"; 
j,  degrees  (angle);  n).; 

point  =  circumference_point  (arc_circle>  angle); 

if  (n  !=  0)  fprintf  (file  ptr,  "  %f  %f\n",  point. x;  point. y)'; 
if  ( (n  !=  0)  Si  (TRACE))  printf  ("  %f  %f\n",  point. x,  point. y); 
if  (n  !=  0)  angle  +=  (rotation  *  delta_angle  /  (double)  n); 
angle  =  normalize2  (angle) ; 

}  '' 

/*  .calculate  and  print  points  for  the  segment  following  the  arc  */ 

point  =  path_ptr->segment.  pointl'; 

fprintf  (file  ptr,  "  %f  %f\n",  point. x,  point;y); 

if  (TRACE)  printf  '("  %f  %f\n",  point. x,  point. y); 


point  ■=■  path_ptr->segment.point2; 

fprintf  (file  ptr,  "  %f  %f\n\"  \n\n";  point. x,  point. y); 
■if  (TRACE)  .printf  ("  %f  %f\n\"  \"\n",  , point. x,  point. y); 


} 

/* 


/* 

if 

( 


) 

/* 

if 


< 


) 


Print  tick  mark  perpendicular  to  start  point  of  current  segment  */ 


( ( (path_ptr->segment.pointl ,x 
(path_ptr->segment . pointl . y 
( (path_ptr->segment. pointl .x 
(pathj?tr->segment. pointl .y 


circle_world->start.x)  (.j, 
circle_world->start.y) )  ss 
circle“world->goaliX)  || 
circle_world-->goal.y) ) ) 


angle  =  orientation  (pathjptr->segment. pointl, 

path_ptr->segment.point2)  +  (PI/2.0); 
xl  —  pathjptr->segmeht. pointl. x  -  (TICK_WIDTH  V  2.0)  *  cos  (angle); 

yl  = -path_ptr->segment. pointl. y  -  (TICK_^WIDTH  /  2.0)  *  sin  (angle); 

x2  =  path_ptr->segment. pointl. x  +  (TICK_WIDTH  /  2.0)  *  cos  (angle); 

y2  -  path  ptr->segment. pointl. y  +  (TICK  WIDTH  /  2.0)  *  sin  (angle); 

fprintf  (?ile_jptr,.  "  %f  %f\n  %f  %f\n\"  \"\n",  xl,  yl,  x2;  y2); 
if  (TRACE)  printf  ("tickmark  at  start  of  current  segment:  \n"); 
if  (TRACE)  printf  ("  %f  %f\n  %f  %f\n\"  \"\n",  xl,  yl,  x2,  y2); 


Print  tick  mark  perpendicular  to  final  point  of  current  segment  */ 
( ( (path_ptr->segment.point2.x  !-  circle_world-7>start.x)  |  | 
(path_ptr->segment.point2.y  !V circle- world->start.y) )  ss 
( (path_ptr->segment.point2.x  !»  circle~world->goal;x)  || 
(path_ptr->segment,point2.y  !=  circle“world->goal.y) ) ) 

angle  =  orientation  (path_ptr->segment. pointl, 

path_ptr->segment.point2)  +  (PI/2.0); 
xl  =path_ptr->segment. point 2.x  -  (TICK_WIDTH  /  2.0)  *  cos  (angle); 

yl  -  path_ptr->segment.point2.y  -  (TICK_WIDTH  /•  2.0)  *  sin  (angle); 

x2  «*  path“ptr->segment.point2.x  +  (TICK_WIDTH  /  2.0);  *  cos  (angle); 

y2  =  path_ptr->segment.point2.y  +  (TICK, WIDTH  /  2.0)  *  sin  (angle); 

fprintf  (file  ptr,  "  %f  %f\n  %f  %f\n\"  I"\n";  xl,  yl,  x2,  y2); 
if  (TRACE)  printf  ("tickmark  at  end  of  current  segment:  \n"); 
if  (TRACE)  printf  (••  %f  %f\n  %f  tf\n\"  \"\n",  xl,  yl,  x2,  y2); 


fclose  (file_ptr); 

if  (TRACE)  printf  ("\n***  graph  path  complete\n") ; 
return; 


*/ 


void  graph_world  (circle_world,  filename) 

/*  Print  circle_world  data  for  unix  'graph'  use,  appended  to  filename  */ 

Circle_world  *circle_world; 
char  *filename; 

( 

E'lLE  *file_ptr; 

int  i,  j;  /*  indices  */ 


Circle_list  *circle_ptr;  /*  index  pointer  to  current  circle  */ 

if  (TRACE)  printf  ("\n***  graph  world  start\n"); 

circlejotr  =  circle_world->circTe_list;  /*  point  to  first  circle  in  world  */ 
if  ((file_ptr  =  fopen  (filename,  "a"))  ==  ((FILE  *)  0)) 
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{ 

error  ("graph_world:  file  open  failure!",  NONFATAL);. 
return; 

} 

if  (TRACE)  printf("  %f  %f\n",  circle^world->start.x,  circle  world->start.y) ; 
if  (TRACE)  printf ("\".  Start\"\n"); 

fprintf  (filejptr,  "  %f  %f\n",  circle_worid->start.x,  circle__world->'s.tart;y)  ;■ 
fprintf.(file_ptr',  StartN^Nn") ; 

if  (TRACE)  printf  (”  %f  %f\n",  circle_world->goai‘.x,  circle  world->goal.y),; 
if  (TRACE)  printf  ("\".  Goal\"\n!,)> 

fprintf (file_ptr,  ”  %f  %f\n",  circle_world->goal.x,  circle_world->goal.y) ; 
fprintf (file_ptr,  Goal\"\n"); 

/*  Loop  to  graph  all  circles  in  circle_world.  */ 

for  (i=l;  i  <=  circle_world->degree;  ++i,  circle_ptr  =  circle:_ptr->next) 

( 

/*  print  current  circle  center  */ 

if  (TRACE)  printf  (“  %f  .%f\n",  circle_ptr->circle. center. x, 

•circle_ptr->circle. center. y) ; 

fprintf  (file_ptr,  "  %f  %f\n",  circle_ptr->circle. center. x, 

circle_ptr->circle. center. y) ; 

if  (TRACE)  printf  ("\".  Circle  %d\"\n",  i);  /*  label  center  w/  circle  */ 
fprintf  (file_ptr,  Circle  %d\"\h",  i);  /*  label  center  w/  circle  */ 

/*  print  circle-  circumference  at  intervals  -  360  /  SUBDIVISIONS  */ 
for  ( j=0;  j  <  360  +  (360  /  SUBDIVISIONS);  j  +=  360  /  SUBDIVISIONS) 

( 

if  (TRACE)  printf  ("  %f  %f\n", 

(circle  6tr->circle. center. x  + 

circle_ptr->circle. radius  *  cos  (j  *  PI  /  180.0)), 

(circle  ptr->circle. center iy  + 

circTejptr->circle. radius  *  sin  (j  *  PI  /  180.0))); 
fprintf  (file_ptr,  "  %f  %f\h", 

(circle  ptr->circle. center. x  + 

circle_ptr7>circle. radius  *  cos  (j  *  PI./  180.0)), 

(circle  ptr->circle. center. y  + 

circTe_ptr->circle. radius  *  sin  (j  *  PI  /  180.0))); 
if  (circle_ptr->circle. radius  -»  0.0) 

break;  /*  only  one  point  needed  in  point  case  */ 

) 

if  (TRACE)  printf  ("\"  \"\nM);  /*  quoted  blank  to  delimit  this  circle  */ 
fprintf  (file  ptr;  "V  \"\n");  /‘  quoted  blank  to  delimit  this  circle  */ 

) 

fclose  (file_ptr)'; 

if  (TRACE)  printf  ("\n“*  graph_world  complete\n") ; 
return; 

) 

/* . . . . . . . . . */ 

void  output_path  (path,  filename) 

/*  Output  path  in  AUV  data  file  format,  appended  to  filename  */ 

Path  ‘path; 
char  ‘filename; 

( 

FILE  ‘file  ptr; 

int  i,  j;  /*  indices  */ 

Path_list  *path_ptr;  /*  index  pointer  to  legs  on  the  path  */ 

if  (TRACE)  printf  ("\n“*  output_path  start \n"); 

path_ptr  =  path->path_list;  /*  point  to  first  leg  of  path  */ 

if  ((file  ptr  =  fopen  (filename,  "a"))  ==  ((FILE  *)  0)) 

< 

error  ("output_path:  file  open  failure!",  NONFATAL); 
return; 

} 

if  (path->label  !=  NULL) 

{ 

fprintf  (file  ptr,  "\nPath  %s\n\n",  path->label) ;  /*  path  header  */ 

if  (TRACE)  printf  ("\nPath  %s\n\n",  path->label) ;  /*  path  header  */ 

} 

else 

( 

fprintf  (file  ptr,  "\nPath  \n\nn); 

if  (TRACE)  printf  ("\nPath  \n\n"); 

} 

/*  print  starting  line  segment  data  */ 
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jfprintf  (filejptr;  "Segment  %8'.2f  %8_.2f  %8.2f  ,%8;2f  %8.2f  %§-2f"/ 
path->initial_segment.pointl-_.x, 
path'->ihitial— segment  .pointl:’.y,. 

DEFAULT_Z, 

path'->initial_.segment.poirit2.x> 

'path->initial_segment .point2.y, 

DEFAULTS); 

if  (TRACE)  printf  ("Segment"  %8;2f  %8.2f  %8.2f  %8.2f-  %8;2f  %8.2f?, 
pathT>initial_segment . pointl.x,  ' i 

path->initial_segment .point l;y, 

. DEFAULT_Z,  » 

path->initial_segment . pdint2 .x, 

„  path->initial_segment:point2.y, 

DEFAULT_Z) ; ' 

pooltime++;  — 

fprintf  (file_ptr,  "  time.-%4.1'f\n">  pooltime); 

if  (TRACE)  printf  ("  time  %4.1f\n"i  pooltime); 

V 

/*  print  all  succeeding  arc  /•  line  segment,  combinations  */ 

for  (i=l;  i  <=  path->degree;  ++i,  pathjptr  “'path_j>tr->next) 

(  . 

if  (path_ptr->arc. rotation  !  =  0)  •/*  don't  print  arc  If  nothing' s- there  */ 

fprintf  (file^ptr;  "Arc  %8.2f  %8.2f  %8.2f  %8.2f.' %8.2f  %8.2f  %2i 

path_ptr->arc. circle. center. x; 
path_pt r->arc . circle . center .  y , 

DEFAULT_Z, 

pa’th_ptrf>arc.  circle,  radius, 
degrees  (normalite2  (path_ptr->arc.anglel) ) , 
degrees  (normalize2  (path_ptr->arc.,angle2) ) , 
pathjptr->arc. rotation) ;  ~ 

pooltime++; 

if  (path  ptr->arc; rotation  ==  CW) 

fprintf  (filejptr,  "='CW-  time  %4.1f^n";  pooltime); 

else  :if  (path_ptr->arc.rotation  “»  CCW) 

fprintf  (file_ptr,  "=  CCW  time  %4.1f\n">.  pooltime) ; 

else  if  (pathjHptr->arc. rotation,  ““-CENTER) 

fprintf  (filejptr,  "»  CENTER  time  %4.1f\n",  pooltime); 

else  fprintf  (file_ptr#  "  time  %4.1f\n",  pooltime);. 

if  (TRACE)  printf  ("Arc  '  %8’.2f  %8.2f  %8.2f  %8.2f  %8.2f  %8.2f  %2i  ", 

-pathjpt  r->arc .circle . center .  x, 
path~ptr->arc. circle. center. y, 

DEFAULT J5, 

path_ptr->arc . ci rcle . radius, 
degrees  (normalize2  (path_jptr->arc.anglel) ) ; 
degrees-. (normalize2  (path_ptr->arc.angle2) ) , 
path_jstr->arc. rotation) ;  ~ 


if 

{ 


) 


) 


(TRACE) 


if  (path_ptr->arc, rotation  CW) 

printf  ,("=  CW  time  %4.1f\n", 

else  if  (path_ptr-r>arc. rotation  CCW) 

printf  ("=  CCW  time  %4.1f\n", 

else  if  (path  ptr->arc. rotation  ==  CENTER) 
printf  ("°  CENTER  time  %4.1f\n", 
else  printf  ("  time  %4.1f\n". 


pooltime) ; 

pooltime) ; 

pooltime) ; 
pooltime) ; 


fprintf  (file_ptr,  "Segment  »8.2f  %8.2f  %8.2f  %8.2f  %8.2f  %8.2f", 
pathjptr->segment. pointl. x, 
path_ptr->segment.pointl.y, 

DEFAULT_Z, 

path_pt r->segment .point2 . x, 
path_ptr->segment . point2 .y , 

DEFAULT_Z) ; 

if  (TRACE)  printf  ("Segment  %8.2f  %8.2f  %8.2f  %8.2f  %8.2f  %8.2f", 
path_ptr->segment .pointl .x, 
path  ptr->segment. pointl. y, 

DEFAULT_Z, 

pathjpt r->segment . point2 . x, 
path_pt r->segment . point2 . y , 

DEFAULT_Z) ; 

pooltime++; 

fprintf  (file_ptr,  "  time  %4.1f\n",  pooltime); 

if  (TRACE)  printf  ("  time  %4.1f\n",  pooltime); 


} 
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fprintf  '(file  ptr,  "\n") ; 

if  (TRACE)  printf  ("An***  output_path  compieteYn") ;’ 
fclose  (file.j?tr); 

•return; 

} 

/* - - - — — — — - - - — - - — - */ 

void  output_world  (circle_world,  filename); 

/*  .Output  circle  world  using  AUV  data  file  format,  appended  to  filename*/ 

Circlejworld  *circle_world; 
char  ‘filename; 

{  ..  “ 

:FILE  *file_ptr; 

iht  i,,  ,j?  /*  indices  */ 

Circle_iist-  *circle_ptr;  .  /*  index-  pointer  to  current  circle  */ 

circle_ptr  =  circle_world->circle_list;  ./*•  point  to  first  circle  in  world  */ 
if  (TRACE)  -printf  ("\n***  -output_world  startNn")  ,* 

if  ((file_ptr  f=  fopen  (filename,  ^a?)  )  ,( (FII£  *)  0)) 

error  ("output_world:  file  open  failure!",  NONFATAL) ; 
return; 

) 

:fprintf  (fi-le_ptr,  . 

"\n*  CircleJHorld  Shortest  Path  Determination\n\n") ; 

fprintf  (file_ptr,  "\nData  specifications  are  according  to  "),- 
fprintf  (fileptr,  "the -AUV  Data  Dictionary. \n\n\n"); 
if  (TRACE) 

{  . 

printf  (  "\n  Circle_World -Shortest  Path  Determination\n\n") ; 

printf  .(  "\nData  specifications  are. according  to  ");- 

printf  (  '.'the- AUV  Data  Dictionary .  \n\n\n") ; 

}  .  '  ‘ 

fprintf  (file_ptr,  "Point  ,%8.2f  %8.2f  %7.2f",  circle_world->start.x, 

circle_world->start.y, 

DEFAULT_Z) ; 

fprintf-  (filejptr,  "  Start\n"); 

if  (TRACE)  printf  ("Point  %8.2f  %8;2f  %7.2£  ",  circle_wo‘rld->start.x, 

circle_world->start.y, 

DEFAULT  Z); 

if  (TRACE)  printf  .("  StartXn''); 

fprintf  (file_ptr,  "Point  %8.2£  %8.2£  %7.2f  ",  circle_world->goal.x, 

circle_world->goal .y, 

DEFAULT_Z) ; 

fprintf  (file.ptr,-  GoalNnVn1!); 

if  (TRACE)  printf  ("Point  %8.2f  %8.2f  %7.2f  ",  circle_world->goal.x, 

circle_world->goal.y, 

DEFAULT  Z); 

if  (TRACE)  printf  ("  Goal\n\n")r 

if  (TRACE)  printf  ("\n***  output_world  circle_world->degree  =  %d  \n", 
circle  world->degree) ; 

for  (i»l;  i  <=  circle_worTd->degree;  ++i,  circle_ptr  =  circle_ptr->next) 

/*  print  circle  center  and  radius  */ 

fprintf  (filejptr,  "Circle  %8.2f  %8.2f  %8.2f  %8.2f  \n", 
circle_ptr->circle. center. x, 
circle_jptr->circle.center.y, 

DEFAULT_Z, 

circle_ptr->circle. radius) ; 

if  (TRACE)  printf  ("Circle  %8.2f  %8.2f  %8.2f  %8.2f  \n", 
circle_ptr->circle . center . x, 
circle_ptr->circle. center. y, 

DEFAULT_Z, 

circle _ptr->circle. radius) ; 

) 

if  (TRACE)  printf  ("\n***  output  world  complete\n") ; 
fprintf  (file_ptr,  "\n");  •  ~ 

fclose  (file_ptr); 
return; 


void  center_graph_window  (filename,  xminptr,  xmaxptr,  yminptr,  ymaxptr, 

magnification) 

/*  Center  (square  off)  the  graph  window  so  printed  circles  aren't  distorted  */ 
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char  ‘filename;  /*  graph  filename  for  input  £. output  */' 

-double  ‘xminptr,  *xmaxptr,  /*  output  values,*  (also  appended  to- file)  */ 

*yminptr,  ‘ymaxptf , 

magnification;-  /*  amount  to- magnify  graph  window- bounds  "*/ 


FILE  ‘filejptr; 

int  col;  t  /*  current  column  being  used  in  line  */ 

char  line  [80];  /*  input  line  string'  of  characters  -*/ 

double  x,  y;  /*  input.  values..from  current  line-  */- 

double  xmin,  xmax,  ymin,  ymax,  /*  min/max  valued"  */ 

del tax,  deitay;  /*  x,  y  max-min  differences  */ 


if  (TRACE)  printf  ("\n***  center_graph_window  ,start\n") ; 

xmin  =  HUGE_VAL;  ./*  Typical  graph -ticks  are  5- units,  apart  */- 

ymin  =  HUGE  VAL; 
xmax  =  -  HUGE'VAL; 
ymax  =  -  HUGE~VAL;. 

if  ((filejptr  =  fopen- (filename,  "r") )  ==  ((FILE  *)  0)) 

( 

error  ("center_graph_window:  file  initial-  open  failure!",  NONFATAL) ; 
return; 

) 

if  (TRACE)  printf  ("\ri***  center_graph_windo'w:  %s  is  open\n">.  filename) ; 
while  ( (fgets-  (line,  81,  filejptr)  !=  NULL))  /*  read  next  line  of  file  */ 
col  =  0; 

while  (line  [col]  ==  '  ')  col++;  ./*  skip  initial- blanks  */ 

if  (isdigit  (line [col] )  I  I  (line  [col]  =■=  '-')  ||  (line  [col]  ==  '+') 

I  j,  (line  [colj  ■==  '  .'■)) 

(  ... 

if  (sscanf  (line+col.d'tlf", tx)  !=  1)  break;  /*  get  x  gracefully*/ 

while  (isdigit  ((int)  line  [col])  ||  (line  [col]  ==  II 

(line  [col]  II  (line  -[colj.  ==  '+')) 

col++;  7*  skip  digits  of  x  */ 

while  ((line [col]  =='  ')■  II  (line  [col]  ==  ',')) 

col++;  '/*  skip  characters  before  y  */ 

if  (sscanf  (line+col,"%l’fn,£y)  !=  1)  break;  /*  get  y  gracefully*/ 
if  (xmin  >  x)  xmin  «  x; 
if.  (ymin  >  y)  ymin  =  y; 
if  (xmax  <  x)  xmax  °  x; 
if  (ymax  <  y)  ymax  =  y; 

if  (TRACE)printf ("\n***  center  graph  window  loop  check:"); 
if  (TRACE) printf  ("  (x,  y)  =  (%6.Jf,  %672f)  ",  x,  y)  ; 

if  (TRACE) printf ("  (xmin,  ymin)  =  (%6.2f,  %6.2f)",  xmin,  ymin); 
if  (TRACE)printf ("  (xmax,  ymax) » (%6.2f,  %6.2f)",  xmax,  ymax); 

)  /*  only  lines  beginning  with  numeric  values  are  checked  */ 

)  /*-  end  while  */ 

if  (TRACE)  printf  ("\n***-  center  graph_window  while  loop  done\n  "); 

if  (TRACE)  printf  ("(xmin,  ymin)“=  (%6.2f,  %6.2f)  ",  xmin,  ymin); 
if  (TRACE)  printf  ("(xmax,  ymax)  =  (%6.2f,  %6.2f).  ",  xmax,  ymax); 

/*  Now  square  off  the  extremes  so  no  distortion  occurs  */ 

if  ((ymax  -  ymin)  <  (xmax  -  xmin))- 

ymax  -  ymin  +  (xmax  -  xmin) ; 
else  if  ((xmax  -  xmin)  <  (ymax  -  ymin)) 
xmax  =  xmin  +  (ymax  -  ymin) ; 

if  (magnification  !=  1.0)  /*  stretch  out  graph  window  boundaries  */ 

deltax  »  xmax  -  xmin; 
deitay  »-ymax  -  ymin; 

xmin  -=  deltax  *  (magnification  -  1.0)  /  2.0; 

xmax  +=  deltax  *  (magnification1  -  1.0)  /  2.0; 

ymin  -=  deitay  *  (magnification  —  1.0)  /  2;0; 

ymax  +»  deitay  *  (magnification  —  1.0)  /  2.0; 


if  (TRACE)  printf  ("\n***  center_graph_window  square-off  "); 
if  (TRACE)  printf  ("and  magnification  complete: \n  "); 

if  (TRACE)  printf  ("(xmin,  ymin)  =  (%6.2f,  %6.2f)  ",  xmin,  ymin); 
if  (TRACE)  printf  ("(xmax,  ymax)  ■=  (%6.2f,  %6.2f)  ",  xmax,  ymax)  ; 
if  (TRACE)  printf  ("\n  "); 

if  (TRACE)  printf  ("magnification  «  %4.2f",  magnification); 

*xminptr  =  xmin;  /*  set  returned  values  using  pointer  indirection  */ 

*yminptr  =  ymin; 
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•xraaxptr  =  xmax; 
•ynaxptr  =  ymax; 


fclose  (file_ptr ) ; 

if  ({file_ptr  »  fopen  (filename,  "a"))  «■«  {{FILE  *)  0)) 

error  ("center_graph_window:  file  re-open  failure! ",  NONFATAL); 
return; 

} 

/*  append  min/max  points  to  file  to  square  off  graph  boundaries  */ 

if  { (xmin  !=  HUGE_VAL).  St  (ymin  !=  HUGEVAL)) 

fprintf  (file_ptr,  "  %8.2f  *8.2f\nV!  \*Tn",  xmin,  ymin);- 
if  ((xmax  !=  -  HOGE_VAL)  it  (ymax  !=  -  HUGE  VAL)) 

fprintf  (file_ptr,  "  18. 2f  %8.2f\n\"  \“Xn",  xmax,  ymax);. 

fclose  (file_ptr); 

if  (TRACE)  printf  ("\n***  center_graph_window  complete\n") ; 
return; 


void  retrieve_circle_world  (circle_world>- 

Circle  world  *circle  world; 

{  “ 

char  filename  [40],  line  [120]; 

FILE  *file_ptr; 

double  x,  y,  r; 

Point  start_point,  goal_point,  center_point; 

Circle  circle; 

if  (TRACE)  printf  ("\n***  retrieve_circle_world  begin\n"); 

printf  ("\n\nEnter  the  name  of  the  circle_world  file  to  retrieve:  "); 
scanf  ("%s",  filename); 

while  ((file_ptr  =  fopen  (filename,  "r"))  ==  ((FILE  *)  0)) 

error  ("retrieve_circle_world  file  open  failure. ..\n  ",  NONFATAL) ; 

printf  ("\nPlease  reenter  the  name  of  the  circle_world  file  "); 
printf  ("to  be  retrieved:  "); 
scanf  ("%s",  filename); 


while- (TRUE)  /*  loop  to  get  start  point  */ 

if  (fscanf  (file_ptr,  "%s",  line)  ■■  EOF)  /*  read  start  point  */ 

error  ("retrieve_circle_world  start  point  read  failure",  NONFATAL); 
return; 

) 

else  if  (strcmp  (line,  "Point")  «■  0) 

{ 

fscanf  (file_ptr,  "Ilf  %lf",  tx,  sy); 
start_point  =  make_point  (x,  y) ; 
if  (TRACE) 

printf  ("\n***  Start  point  »  (%4;2f,  %4.2f)\n",  x,  y); 
break; 

) 

) 

while  (TRUE)  /*  loop  to  get  goal  point  */ 

{ 

if  (fscanf  (file_ptr,  "Is",  line)  ==  EOF)  /*  read  goal  point  */ 

error  ("retrieve_circle_world  goal  point  read  failure",  NONFATAL); 
return; 

) 

else  if  (strcmp  (line,  "Point")  *>■»  0) 

( 

fscanf  (file_ptr,  "Ilf  Ilf",  Sx,  Sy); 
goal_point  -  make_point  (x,  y) ; 
if  (TRACE) 

printf  ("\n***  Goal  point  »  (14. 2f,  I4,2f)\n",  x,  y) ; 
break; 

} 

) 

create_circle_world  (start_point,  goal_point,  circle_world) ; 

while  (TRUE)  /*  loop  to  get  next  circle  */ 

( 

if  (fscanf  (file_ptr,  "Is",  line)  “  EOF)  /*  read  next  circle  */ 
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{ 

break; 

} 

else  if  (strcmp  (line,  ^Circle")  ==  0) 

{ 

fscanf  (file_ptr,  "%lf  %lf  %*lf  %lf",  Sx,  Sy,  Sr); 
center_point  «  make_point  (x,  ;y);‘ 
circle-  =  make_circle  (center  point,  r) ; 

if  (TRACE) 

printf  ("\n***  Circle  »  (%4.2f,  %4.2f,  %4.2f)\n", 
add  circle  to  world  (circle,  circle  world); 

)  "  "  " 

) 

if  (TRACE) 

( 

printf  ,("\n***  circle_world  start  point  ■  (%4.2f,  %4.2f)\n!', 
circle  world->start.x,  circle_worlc^->start.y) ; 
printf  ("\n***  circle_world  goal  point  »  (%4.2f,  %4.2f)\n", 
circle_world->goal.x,  circle_world->goal,y) ; 
printf  ("\n***  circle_world  degree  "  »  ld\n", 
circle  world->degree) ; 

) 

if  (TRACE)  printf  ("\n***  retrieve_circle_world  complete\n") ; 
fclose  (filejptr); 

return; 

) 

/* . 


x,  y,  r); 


*/ 
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/****■*»  List  0f  circie  search  Data  Structures  and  Functions  *******************/ 
/* 


Data  Structures 

Data  Types  and  Member  Labels 

Matrix 

nam^ 

Tangent_matrix_element 

int 

double 

double 

double 

visible; 

segment_cost; 

leaving^angle; 

landlng~angle; 

tangents  [n] [n) 

Search_matrix_element 

int 

int 

double 

double 

mark; 

predecessor; 

least_cost; 

landing_angle; 

search 

[n) 

Functions 

Parameters 

initialize_tangents_and_search_matrices  (search_type,  circle_world, 

all_tangents) 

update_unmarked_Xeast_costs  (marked_node,  circle_world) 

unmarked_vertices 

unmarked_circle_with_minimum_cost  (search_type,  plot_each_leg, 

circle  world) 
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[includes  A-star -evaluation  function 
of  arc  £  segment  distance,  to  goal) : 

build_best_path  (circle__world) 

circle_search  (search_type,  plot_each_leg', 

circle_world,'  -  — 

best_path,  all_tangents) 


*/ 


/***««*  Circle_search  Data  Structures,  Type  Definitions  £  Global  Variables  *♦**/ 
/* - - - - - - - - - - - - — — - --*/ 


typedef  struct  Tangent_matrix  element_type 

{ 

int  visible;  /*  whether  segment  is  VISIBLE,  NONVISIBLE, 

I*  or  TANGENTIAL 


double 

double 

double 

) 


segment_cost;  /*  cost  of  this  tangent  segment 
leaving“angle;  /*  angle  leaving  from  i_th  circle 

landing_angle;  /*  angle  landing  on  '  j~th  circle 


Tangent_matrix_islement; 


/* 


*/ 

*/ 

*/ 

*/ 

*/ 


*/ 


static  Tangent_matrix_element  tangents  [2*MAX_CIRCLES+1) (2*MAX_CIRCLES+1) ; 


/*  tangents  */  /*  square  global  tangent_matrix  of  tangents  and  costs  */ 

/*  zeroth  element  corresponds  to  Start  point  S  */ 

/*  first  element  corresponds  to  Goal  point  G  */ 

/*  second  element  corresponds  to  circle  1  CW  side.  ?/ 

/*  third  element  corresponds  to  circle  1  CCW  side  */ 

/*  (2n)  element  corresponds  to  circle  d  CW  side  */ 

/*  (2n+l)  element  corresponds  to  circle  d  CCW  side  */ 

/*  where  d  »  circle_world->degree  */ 

/*  column  i  circles  are  the  leaving  circles  */ 

/*  row  j  circles  are  the  landing  circles  */ 

/*  circle  CW  sides  have  even  indices  */ 

/*  circle  CCW  sides  have  odd  indices  */ 

/*  Note  that  travel  is  impossible  between  the  */ 

/*  CW  £  CCW  sides  of  an  individual  circle.  */ 

/*  Also  note  .that  tangents  [i)[j)  and  tangents  [j)[i]  */ 

/*  are  determined  independently.  This  allows  the  use  */ 

/*  of  a  directionally  dependent  cost  function  */ 

/*  without  loss  of  generality.  */ 


/* 


*/ 


/* - - */ 

typedef  struct  Search_matrix_element_type 

int  mark;  /*  boolean  whether  shortest  path  is  found  */ 

int  predecessor;  /*  circle  or  (start  point)  preceding  circle  */ 

double  least_cost;  /*  least  cost  found  so  far  to  this  circle  */ 

double  landing_angle;  /*  angle  landing  on  this  circle  */ 

Search_matrix_element; 

/* - ,/ 

static  Search_matrix_element  search  [2*HAX_CIRCLES+1] ; 

/*  search  */  /*  matrix  used  to  perform  single  source  shortest  path  */ 

/*  search  (Dijkstra's  algorithm)  */ 

/* - */ 

static  int  n;  /*  Number  of  active  indices  in  tangents  £  search  matrices  */ 

/*  n  =  2  *  circle_world->degree  +  1;  */ 

static  int  total  visible; 
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/*  .total  number  of  tangents  visible  in  the  set  of  all  */ 

/*  ’  possible, external  ‘S  cross-tangents,,  between;.- the  */ 

•/*  start  point,  goal  point  and  circles  iri :  circle_world  */ 

static  int  leg_number  =0;  . 

”  /*  occurrence  number  of  latest  leg  with  least  cost  */' 

/*---- - - T-r-t-rTr-T-— - - . - ----*/ 


/***»»*  circle  search  Function  Declarations  ***********************************/ 


/* - - - ; - - - r - -— - — — — - - - - - - r - ---*/ 

void  initialize_tangents  and_search_matrices  (search_type,  circle_world, 

all^tangents) 

int  search_type;  /*  DIJKSTRA  or  A_STAR  */ 

Circle_world  *circle~world; 

Path  -  *all_tangents; 

int  i,  j,  model,  mode2;  /*  indices  and  rotation  modes  */ 

int  START  GOAL,  visible2;  /*  logical  checks  */ 

double  angle?  /*  working  variables  */ 

Point  pointl,  point2; 

Segment  segment; 

Circle  circleO,  circlel,  circle2; 

Arc  arcO; 

Configuration  *confi.gl,  *config2; 


pointl. x  «  circle_world->start.x  -  0,3;  /*  offset  x,  y  for  tangents  label  */ 
pointl. y  ■  circle“world->start.y  -  0.8; 
segment  -  make_segment  (pointl,  pointl); 

all_tangents->label  «  "  All  visible  tangents  between  circles  included"; 

all~tangents->degree  ■  0; 

all-tangents->initial  segment  »  segment; 

all~taugents->path_list  ■  ((Path_list  *)  0);  /*  NULL  */ 

configl  ■  (Configuration  *)  malloc  (sizeof (Configuration) ) ; 
config2  ■  (Configuration  *)  malloc  (sizeof (Configuration) ) ; 

total_visible  -  0;  /*  initialize  visible  tangent  count  */ 


/* - - - - - -  Tangents  matrix  initialization  - - •-*/ 

if  (TRACE) 

{ 

printf  ("\n\n"); 

print f  (" _ "); 

printf  ("  _ _ _ "); 

printf  ("\n\n" ) ; 

) 

printf ("\n\nCommencing  least  cost  path  determination  using  "); 
if  (search_type  >«»  DIJKSTRA)  printf  ("Dijkstra  "); 

else  if  (search~type  ■»  A_STAR)  printf  ("A-star  "); 
else  ~  “  printf  ("circle_") ; 

printf ("search. . .\n\n\n") ; 

for  (i  •>  START;  i  <«=  h;  ++i) 

( 

if  (i  »«  START) 

circlel  «=  make_circle  (circle_world->start,  0.0); 
else  if  (i  =«  GOAL) 

circlel  «  make_circle  (circle  world->goal,  0.0); 
else  circlel  =  find~circle  (i/2,  cTrcle_world) ; 

if  ( (i  ==  START)  ||  (i  ==  GOAD) 

model  •=  CENTER;  /*  circle  world  start  {  goal  points  */ 

else  if  (i  -=  (i  /  2)  *  2) 

model  =  CW;  /*  even  »>  CW  side  of  circle  */ 

el  se 

model  =  CCW;  /*  odd  =>  CCW  side  of  circle  */ 

if  (TRACE)  printf  ("\n _ Tangent _ "); 

if  ((TRACE  ==  TRUE)  ii  (i  >=  10))  printf  ("  "); 
if  (TRACE)  printf  ( 

"  Visibility  _ Circles _ _ Coordinates _ Cost_"); 

for  (j  =  START;  j  <=  n;  ++j)  /* - */ 

{ 
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if  (j  ==  START) 

,  -circled  “  make. -circle  (circle  w6rld->start,  0:  0)  ; 
else  if  (j  ==  GOAL)  ~ 

circle2;  = ’make_circle.  (circle  world->goal,  6.0); 
else  circle2  =  find“circle  ( j/2, '  clrcle^world) ;, 

if  ((j  ==  START):  ||  (j~  ==  GOAL)) 

mode2  =  CENTER;  /*  circle  world  start  &  goal  points  */ 
else  if  (j  ==  (j  /  2)  * 

mode2  -  CW;  /*  even  =>  CW  side-of,  circle  *./ 

else 

mode2  =  CCW;  ■  /*  odd  =>  CCW  side-of  circle  */ 


,/*---- - .. - --- - - r - --r— :-r--- - - - -----*/ 


if  ((i  —  START  it  j  —  GOAL)  || 

( 

START_GOAL  =  TRUE; 
tangents  [i](  j] .  segment_cost 

) 

else  START_GOAL  -  FALSE; 


(i  —  GOAL  it  j  —  START)) 

«  distance  (circle_world->start, 
circle“world->goal) ; 


if  ((i/2  —  j/2)  (START_GOAL  —  FALSE)) 

/*  IMPOSSIBLE  diagonal  case:  same  circle,  Start-Start,  Goal-Goal  */ 
tangents  (i) [j] .visible  ■  NONVISIBLE; 

if  (  ( (i  GOAL)  S  S  ( j  —  GOAL))  |j  ((i  -=  START)  SS(j  —  START))) 
tangents  [i) ( j] ,segment_cost  -  6.0; 
else 

tangents  [i] ( j) . segment_cost  »  HUGE  VAL; 
tangents  (i]  [  j) . leaving_angle  -  0.0;r 
tangents  (i) ( j) ,landing_angle  -0.0; 

else  /*  all  other  non-IMPOSSIBLE  cases  */ 

circle_tangent  (circlel,  circle2,  model,  mode2,  configl,  config2); 

pointl  ■  circumference_point  (circlel,  configl->tangent. angle) ; 
point2  -  circumference^point  (circle2,  conf ig2->tangent. angle) ; 
segment  -  make_segment  Tpointl,  point2); 

tangents  (i) (j) .visible  -  visible  (pointl,  point2,  circle_world) ; 

/*  Calculate  costs  if  VISIBLE  t  save  segment  cost  value  */ 

if  (  (tangents  (i) (j) .visible  —  VISIBLE)  || 

^  ((tangents  (i)(j) .visible  —  TANGENTIAL)  it  TANGENTS_OK) ) 

tangents  (i) ( j) ,segment_cost  «  segment  cost  (segment); 
total_visible++; 

/*  add  a  segment  to  path  containing  only  tangents  */ 

angle  »  orientation  (pointl,  point2)  +  (PI/2.0); 

circleO  ■  make_circle  (pointl,  0.0); 

arcO  -  make~arc  (circleO,  angle,  angle,  0) ; 

augment_path  (arc07  segment,  all_tangents) ; 


else  /*  segment  is  NONVISIBLE,  no  cost  calculations  required  */ 
^  tangents  [1] [ j) . segment_cost  »  HUGE_VAL; 

if  (j  «»  GOAL)  /*  get  distance-to-goal  A*  evaluation  function  */ 
/*  regardless  of  visibility  */ 

tangents  (i) ( j) . segment_cost  -  segment_cost  (segment); 

tangents  [i] [ j) .leaving_angle  =  conf igl->tangent. angle; 
tangents  [ij ( jj . landing_angle  =  conf ig2->tangent. angle; 


if  (TRACE) 

< 

printf ("\n***  tangent  [%d] [%d]  ",  i,  j); 

if  (j  <  10)  printf  ("  "); 

if  ((i/2  ==  j/2)  (S  (START  GOAL  ==  FALSE)) 

printf  ("IMPOSSIBLE") ;“ 

else  if  (tangents  [i]  [j} .visible  ==  VISIBLE) 
printf  ("  ■  VISIBLE"); 

else  if  (tangents  [i) tj) .visible  ==  NONVISIBLE) 
printf  ("NONVISIBLE"); 

else  if  (tangents  [i] (j) .visible  ==  TANGENTIAL) 
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printf  ("TANGENTIAL")'; 

if  (i  ==  0)  .printf  ("  S  ") ,\  /*  leaving- circle*.#  */ 

else- if  (i  ==  1)  -printf  (V  G,  "); 

else  .printf  ("Id  7/  i/2); 

if  (model  ?=  CW)-  printf-  (,',' _CW: ;  ") ; 

else  if  (model  ==  CCW)  printf  ("CCW.-.").; 

else  if  (model  ==  CENTER)  printf  ("PT.."); 

else  ,  printf  -(!'  ",) ; 

if  (j  ==  0)*  printf  ("S' 7);  /*  landing,  circle  #  *■'/ 

else  if  ( j-  ==  1)  printf  ("G-. 

else  printf  (’«%d  ",.  j/2);  - 

if  (mode2  CW)  printf  ("CW.-");. 

else  if  (mode2  ==  CCW)  printf  ("CCW"); 

else  if  (m’ode2  ==  CENTER)  printf  { "PT  "); 

'else  printf  ("  "); 

if  ((i/2  !“  j/2)  ||  (START.  GOAL  ==  TRUE)) 

{' 

printf  ("  (%5. 2f , %5.2f ).. . (%5.2f,  %5.2f) ", 

pointl.x,  pointl.y,  point2.x,  point2.y); 

if  ((  tangents  (i)  (j)  .visible  -*■  VISIBLE)  II 
(  START  GOAL  —  TRUE)  | | 

((tangents  (i) (j). visible  =»  TANGENTIAL)  it  TANGENTS  OK)  || 
((tangents  (i)  (j) .visible  =«  NONVISIBLE)  it  (j  -»  GOAL) ) ) 
printf  ("  %4,lf",  tangents  [i] (j) .segment  cost); 

) 

/*  Change  TANGENTIAL  to  VISIBLE  or  NONVISIBLE  as  appropriate  */ 
if  ((tangents  (i) (j) .visible  ==  TANGENTIAL)  it  TANGENTS_OK) 
tangents  jij (j) .visible  »  VISIBLE; 
else  if  (tangents  (i) (j) .visible  »“  TANGENTIAL) 
tangents  (ij.(  j)  .visible  =  NONVISIBLE; 

) 

)  /*  for  j  loop  complete  */ 

if  (TRACE)  printf  ("\n") ; 

-}  /*  for  i  loop  complete  */ 

/* - - - Search  matrix  initialization - - - */ 


for  (i  -  START;  i  <-  n;  ++i) 

*  search  (i).mark  -  UNMARKED; 

search  (i) .predecessor  »  NULL;  /*  impossible  initialization  value  */ 
search  (ij ,least_cost  »  HUGE_VAL; 
search  (i). landing  angle  «  0.0;~ 

)  '  " 

search  (START) .mark  «  MARKED;  /*  Start  point  found  by  definition  */ 

search  (START) .predecessor  »  START;  /*  initialize  remaining. slots  */ 

search  (START) ,least_cost  »  0.0; 

search  (START) .landing_angle  «  0.0; 

printf  ("\n\nTangent  and  search  matrix  initializations  are  complete. \n") ; 
printf  ("\n\n%d  out  of  %d  potential  tangents  (%3.1f  %%)  are  usable. \n\n", 
total  visible,  (n  *  n  +  1), 

100.0”*  (float)  total_visible  /  (float)  (n  *  n  +  1)); 

return; 

) 

/* . */ 


void  update_unmarked_least_costs  (marked_node,  circle_world) 


{ 


int 

marked_node; 

Circle_world 

*circle~world; 

int 

new, 

/* 

index  for  checking  each  new  node 

*/ 

rotationl, 

rotation2. 

/* 

rotations  for  first  i  second  circles 

*/ 

sign_second_arc; 

/* 

MINUS  PLUS  or  ZERO 

*/ 

double 

new  cost, 
angXel, 

/* 

marked  circle  landing  angle 

*/ 

angle2. 

/* 

marked  circle  leaving  angle 

*/ 

angle3. 

/* 

new  circle  landing  angle  best  so  far 

*/ 

angle4; 

/* 

new  circle  landing  angle  latest 

*/ 

Arc 

first_arc. 

second_arc; 

Circle 

first”arc_circle. 

second_arc_circle; 

if  (TRACE) 

( 
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printf  ("\n***  update_unmarked^least_costs  visible  from  node  %d  '!/, 
•marked_no'de) 

if  '(marked_node  --  START) 

printf  (start  .point) ») ; 
else  if  (marked_node  ==  GOAL) 

printf  (J  (goal  point)  ") 

else  printf  ("(circle  %d",  (marked  node/2).);, 
if  ((marked  node  !=  GOAL)  a  (marKed_node  !=  START)  SS 

( (narked_node/2) *2  =*=  marked_node) ) 
printf  .(""  CW  left  side)"); 

else  if  ( (marked^node  !=  GOAL)  SS  (marked  node  !=  START)') 
printf  ("-CCW  right  side)"); 
printf  (":"); 


for  (new  =  GpAL;  new  <=  n;  ++new).  /*  loop  through  all  elements  */ 

/*  Calculate  total  cost  to  new  circle  via  marked_node  circle  */ 

if  (marked_node  ==  START) 

first  arc  circle  »  make_circle  (circle_wprld-i,'start,  0.0); 
else  if  (marke3_no3e  »«  GOAL)  ~  ; 

first  arc  circle  ■  make_circle  (circle_^wdrld->goal,  0.0); 
else  first~arc~circle  ■  find~circle  (marked~node/2,  circle_world) ; 

anglel  »  search  [marked_node] . lahding_angle; 
angle2  “tangents  [marked“nodej (new) ,leaving_angle; 

if  (marked  node  ==  (marked  node  /  2)  *  2) 

rotationl  =  CW;  “  /*  even  node  =>  circle  CW  side  */ 

else 

rotationl  »  CCW;  /*  odd  node  »>  circle  CCW  side  */ 

first_arc  »  make_arc  (first_arc_circle,  anglel,  angle2,  rotationl); 

/*  Calculate  cost  difference  due  to  different  landing  points  */ 

if  (new  “■  START) 

second_arc^circle  ■  make_circle  (circle_world->start,  0,0); 
else  if  (new  -"“GOAL)  -  .  - 

second_arc_circle  -  make_circle  (circle_world-  goal,  0.0); 
else  second~arc_circle  -  find~circle  (new/2,  circle_world) ; 

angle3  =  search  (new) .landing  angle;  /*  prior  best  angle  */ 

angle4  »  tangents  (markedjnodejfnew) ,landing_angie;  /*  latest  angle  */ 

if  (new  »»  (new  /  2)  *  2) 

rotation2  ■  CW;  '/*  even  node  »>  circle  CW  side  */ 

else 

rotation2  »  CCW;  /*  odd  node  »>  circle  CCW  side  */ 

second_arc  =  make_arc  (second_arc_circle,  angles,  angle4,  rotation2); 

if  ((search  (new) .predecessor  ==  NULL)  || 

(search  (new) .predecessor  ««  START)) 

sign_second_arc  =  EERO;  /*  no  arc  exists  if  no  predecessor  */ 

else  if  ((precede  (angle4,  angle3)  it  (rotation2  =*=  CW  ) )  II 

(precede  (angle3,  angle4)  it  (rotation2  »“  CCW))) 

sign_second_arc  -  MINUS; /*this  arc_cost  applies  to  prior  best*/ 

else 

sign_second_arc  =  PLUS;  /*this  arc_cost  applies  to  current  */ 


/* - - - */ 

/*  After  all  this  work  we  finally  can  add  up  costs  for  comparison  */ 

new_cost  =  search  (marked_node) ,least_cost  + 
fabs  (arc  cost  (first  arc))  + 
tangents  T»arked_nodeT(new) .segment_cost; 


/* - */ 

if  (TRACE) 

{ 


if 

(new  =“  GOAL)  printf  ("\n 

Goal: 

",  new); 

else 

printf  ("\n 

node 

%d:  ",  new); 

if 

(new  <  10)  printf  ("  "); 

if 

( (marked  node/2  ==  new/2)  It 

(new 

!=  START) 

a 

(new 

!=  GOAD) 

printf  ("IMPOSSIBLE"); 
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else  if  (tangents  [marked_node]  [new]  .visible  =■=  VISIBLE) 
pfintf  ("  VISIBLE") ; 

else  if  (tangents  [marked_hode]  [new]  .'visible  ==  NONVISIBLE) 

.printf  ("NONVISIBLE");  . . 

else  if  .'(tangents  [marked_node]  [new]  .visible  ==  TANGENTIAL)' 
printf’ ("TANGENTIAL"); 

if  (search,-  (new)  .mark  “  MARKED)  printf  ("  MARhSD"),;, 

else  if  (search  (new) .mark  ==  UNMARKED)  printf  ("  UNMARKED") ; 
printf  ("  prior  best  cost  ");- 

if  ( (search*  [new]  .least^cost  >»  0.0)  ts 

(search  [new]  .leastxost  <  10.0)  )  printf  ("  ").; 

if  ((search  [new]  . least_cost  ==  <HUGE_VAL) )'  printf  ("  ")  ; 

printf  ("%f",  -search  [new]  .least^cost)  ;■ 

if  ( (search  [new] .least_cost  ==  HUGE_VAL) )  printf  >("inite".); 

if  ((tangents  [marked'.nbde]  [new]  .visible  ■*=  VISIBLE)  && 

(search  [new]  .marie  “  UNMARKED) ) 

( 

printf  (",  new  cost",); 
if  ((new  cost  >“  0.0).  && 

(riew_cost  +  (Tabs  (arc  cost  (second_arc)’)  *  sign  second  arc  <10.0))) 
~  prTntf  ("~"); 

if  (new  cost  -=  HUGE_VAL)  printf  ("  "); 

printf  T"*f", 

new_cost  +  (fabs  (arc  cost  (second’ arc) )  *  sign_second_arc) ) ; 
if  (hew  cost  -■*  HUGE  VALT  printf  ("inTte");. 

) 

} 

/* . . . r - — -r - — - - - */ 

/*  Compare  and’  replace  if  new_cost  is  better  than  current  cost  */ 

if  ((tangents  [marked  node] [new] .visible  »»  VISIBLE)  && 

(search  [new]. marie  »■  UNMARKED)  it 

(new_cost  >•>  0.0)  a 
(search  (new). least  cost  > 

new  cost  +  (Tabs  (arc  cost  (second  arc) )  *  sign  second  arc) ) ) 
{  ~  “  “ 
search  (new) .least_cost  »  new  cost; 

search  [new] .predecessor  ■  marRed^node; 

search  [new] .landing_angle  - 

tangents  [marked_node] [new] .landing_angle; 

)  . 

if  (TRACE) 

{ 

printf  ("\n***  update_unmarked_least_costs  via  node  %d  ",  marked_node) ; 
if  (marked_node  START) 

printf- (" (start  point)"); 
else  if  (marked_nocie  **■  GOAL) 

printf  ("(goal  point)"); 

else  printf  ("(circle  %d",  (marked  node/2)); 
if  ((marked  node  !“  GOAL)  a  (mar)ied_node  !=  START)  &£ 

( (marke3_node/2)  *2  ==  mariced_node) ) 
printf  (■"  CW  left  side)");  — 
else  if  ((marked  node  !-  GOAL)  a  (marked  node  !»  START)) 
printf  7"  CCW  right  side)"); 
printf  ("  complete. \n") ; 


return; 

) 

/» - *i 

int  unmarked_vertices  ()  /*  boolean  value  for  remaining  unmarked  vertices  */ 

1 

int  i; 

for  (i  =  GOAL;  i  <=  n;  ++i)  /*  Don't  check  START  0,  default  is  MARKED  */ 

{ 

if  (search  (i).mark  ==  UNMARKED) 

{ 

if  (TRACE) 

( 

printf  ("\n***  unmarked_vertices  check  result  ■=  TRUE;  "); 
printf  ("unmarked  vertices  exist. \n"); 

} 

return  TRUE; 
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}, 

> 

if  (TRACE) 

{  ,  .  ,,  ... 

printf  ("\n***  unmarked_vertices  .check  result  =  FALSE;  "!; 
printf  ("no  unmarked  vertices  remain;");; 

) 

return -FALSE; 


int  unmarked_circle_with_mihimum_cost  (search_type, .plot_each_leg,  circle_world) 

iiit  search_.type,  /*  DIJKSTRA  or  AjSTAR  */ 

plot  each_leg;  /*  TRUE  .or- FALSE'  */ 

Circl‘e_world  *circTe_world;'  /*  input: all  circles  */ 

/*  includes -A-star  evaluation  function  of  arc  S  segment  distance  to  gpal  */ 


-( 


int 

double 
static  char 
Point 
Circle 
Arc 

Segment 

Configuration 

Path 


i,  least_circle,  /*  working  variables,  declaration 

model,  mode2,  last,  last2; 

min  cost,  evall,  eval2; 

label  [10);, 

pointl,  point2; 

circlel,  ci'rcle2; 

arcl,arc2; 

segment; 

*configl,*config2> 

*current_leg;, 


*/. 


configl  “  (Configuration  *)  malloc  (sizeof  (Configuration)); 
config2  -  (Configuration  *)  malloc  (sizeof  (Configuration)); 


min_cost  »  HUGE_VAL; 

least_circle  =  START;"  /*  this  initial  value  isn't  possible*/ 


if  (TRACE) 

printf  ("\n***  unmarked_circle_with_minimum_cost  determination:"); 

for  (i  ■  GOAL;  i  <»  n;  ++i)  /*  Don't  check  START  0,  default  is  MARKED  */ 

{ 


if  (search  type  •*■  A  STAR)  /*  then  calculate  evaluation  functions  */ 

< 

/* —  Determine  evall:  previous  best  min_cost  circle  "least_circle"  */ 

last  -  search  (least_circle) .predecessor; 
if  (least_circle”-»  START) 

circlel  -  make_circle  (circle_world->start,  0.0); 
else  if  (least_circle  »«  GOAL)  - 

circlel  »make_circle  (circle_world->goal,  0.0); 
else  circlel  =  find_circle  (least_circle/2,  circle_world) ; 

if  ((least  circle  —  START)  ||  (least  circle  ■■  GOAL)) 

model  =  CENTER; 

else  if  (least_circle  ==  (least_circle/2)  *  2) 
model-”  CW; 
else  model  *»  CCW; 


arcl  =  make_arc  (circlel, 

tangents  [last] [least_circle) .landing_angle, 
tangents  [least  circle] [GOAL] .leaving_angle,  model); 
evall  *»  min_cost  +  arc_cost  (arcT)  + 

tangents  [least_circle] [GOAL] .segment_cost; 

/* —  Determine  eval2:  current  min_cost  circle  "1"  - */ 

last2  “  search  [i] .predecessor; 
if  (i  -=  START) 

circle2  “  make_circle  (circle_world->start,  0.0); 
else  if  (i  ==  GOAL) 

circle2  ■=  make_circle  (circle  world->goal,  0.0); 

else  circle2  =  find-circle  (i/2,  cTrcle_world) ; 

if  ((i  «  START)  II  (i  «»  GOAL)) 

mode2  =  CENTER; 
else  if  (i  ==  (i/2)  *  2) 
mode2  =  CW; 
else  mode2  =  CCW; 


arc2  =  make_arc  (circle2. 
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•tangents  ,[last2]'[l]  .landing^arigie, 
tangents  ,[  i][GOAL] .  leaving_angle 'mode2).;-‘ 
eval2'  =  search  [i] ; least_cost  +  arc_cost  (arc2)-‘  + 
tangents \[i-]-[GOAL]  ;  segment  ••.cost;' 

}  '  . 

/*  Determine  if  current  circle  beats  previous>Best  miri_cost., circle.  ---*/•• 

if  (search, (i). mark  ==  UNMARKED)  ,/*  Evaluate  which-.node  has  min  .cost  */ 

<  / 

if  ( ( (search_^type— DIJKSTRA)  -SS  (min_cost  >_ search  [i'JYleast^cqst) )’ 

||  ( (search~type==A_STAR)  SS  (miri_cost! -HUGE_VAL)  SS(evall>eval2)) 

•  II'  ( (search-type==A_STAR)  SS  (min-cost==HUGE~VAL)  SS 

(search  [i'hleast  cost  !=  HUGE  VAL)):)' 

<  . 

min__cost  »  search-  [ij  .least_cost; 
least_circle  =  i  ; 

) 

}  ...  ... 

/* —  TRACE  statements  for  each  loop, - — — r-7 — rr - - -*/ 

if  ((TRACE)  SS  (search'  (i] .mark' r-  UNMARKED)) 

{ 

if  (i  —  GOAL)  printf  ("\n  Goal:  «!,  i); 

else  printf  (,"\n  node  %d:  ",  i); 

if  (i  .<  10)'  printf  ("  "); 

if  (search  type  =*»  DIJKSTRA) 

( 

printf  ("current  cost  "); 
if  ((search  [i] .least_cost  >»  0.0)  ts 

(search  [i)  .least-cost  <  10.0))  printf  ("'"); 

if  (search  [i]  .least_cost  *'■  HUGE_VAL)  printf  ("  "); 
printf  ("%f";  search  [i] .least_cost) ;— 

if  (search  [i] ; least  cost  *>“  HUGE  VAL)  printf  ("inite"); 

):  T  ' 

else  7*  A,  STAR  */ 

{  "  • 

printf  ("distance  to  goal  "); 

if  ((tangents  [1  ]  [GOAL] . segment_cost  +  arc_cost  (arc2)  >■=  0.0)  SS 

(tangents  [ij  (GOALj,.segment-cost  +  ,arc~cost  (arc2)  <  10.0)  ) 

printf  ("  "); 

if  (tangents  (i)  (GOAL),. segment  cost—  HUGE  VAL) 
printf  ("  "); 

printf  ("%f",  tangents  [i } [GOAL) .segment  cost  +  arc  cost  (arc2)); 
if  (tangents  [i),(G0ALJ  .segment  cost—  HUGE  VAL)~ 
printf  ("inite"); 

printf  (",  current  evaluation  function  "); 
if  ( (eval2  >-  0.0)  SS  (eval2  <  10; 0) )  printf  (•'  "); 
if  ( (eval2  —  HUGE_VAL) )  printf  ("  "); 

printf  ("%f",  eval2); 

if  (eval2  —  HUGE_VAL)  printf  ("inite"); 

printf  ("\narc  cost  (arc2)  ■  If",  arc  cost  (arc2)); 

)  “ 

) 

) 

/* - - - ;-*/ 

if  (plot_each_leg  —  TRUE)  /*  plot  this  minimum  cost  leg  in  graph  file  */ 

last  »  search  (least_circle) .predecessor; 
if  (last  —  START) 

circlel  =  make  circle  (circle  world->start,  0.0); 
else  if  (last  ==  GOAL)  ~ 

circlel  =  make_circle  (circle_world->goal,  0.0); 
else  circlel  »  find_circle  (last/2,  circle_world) ; 

if  ((last  —  START)  ||  (last  —  GOAL)) 

model  »  CENTER; 

else  if  (last  —  (last/2)  *  2). 

model  »  CW; 
else  model  »  CCW; 

if  (least_circle  ==  START) 

circle2  «  make_circle  (circle_world->start,  0.0); 
else  if  (least_eircle  —  GOAL) 

circle2  =  make_circle  (circle_world->goal,  0.0); 

else  circle2  =  find~circle  (least_circle/2,  circle_world) ; 

if  ( (least  circle  —  START)  1 1  (least  circle  —  GOAL) ) 

mode2  =  CENTER; 

else  if  (least_circle  ==  (least_circle/2)  *  2) 
mode2~=  CW; 
else  mode2  =  CCW; 
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circle  tangent  (circlel,  circle2‘,  model,  mode2,  configl;  .config2); 
circleT  =  conflgl->tarigent. circle; 
circle2  -  config2->tapgent. circle; 

point!  -  circumference Jpoint  (circlel,  cohfigl->.tangent. angle) 
point2  =  circumference_point  (circle'2,.  conf ig2r>tangent . angle )  ; 
segment  =  make_segment  (point!;-  ,point2)  ; 

-current  leg.  =  create_path  (segment),; 
leg,  numEer++; 

sprTntf' (label,  "leg  %d",  leg_number) ; 
current_leg->label  =  label;, 

grapbjpath  (current_leg,  ’  circle_world,  GRAPH_FILENAME) ; 


/*-  TRACE  statements  for  completion  . - — - - - - ; - */ 

if  .(TRACE) 

{  ,  ' 

printf  ("\n***  unmarked_circle_with_minimum_cost :  node  %d",, 
least_circle) ; 

printf  ("  with  current  cost  %f;",  min_cost); 
printf,  ('JVn  ~ 

if  (least  circle  *>“  GOAL)  printf  ("(GOAL)"); 
else  -  -printf  ("  "); 

printf  ("  node  %d  is  now -marked  and  has-  node- %d", 

least  circle,  search  [least_circle)  .predecessor)  ;■ 
printf  ("  as- its  pre3ecessor,\n") ; 

) 

return  least  circle; 

) 

- - - - - -.—---7 - - - - - - - */. 


void  *build_best_path-  (circle_world,  best_path) 


Circle_world  *circie_world; 
Path  -  *best_path; 


( 


/*  work  from  goal’  to  start  £  buiid  path  */ 
/*  using  predecessor  t- least-cost  */ 
•/*  results  from  circle^searcK  */■ 


int  circle,  predecessor, 

current,  rotation; 
double  anglel,  angle2; 

static  char  label2  (40); 


Point  pointl,  point2; 

Segment  segment; 

Circle  circlel,  circle2; 

Path_list  *path_ptr,  *new_leg; 

if  (TRACE) 

( 

printf  ("\n\n"); 

printf  (" _  "); 

printf  ("  ,  _ "); 

printf  ("\n\n") ; 

printf ("\n***build_best_path:  work  backwards  from  goal  to  start. \n");- 


best_path->path_list  NULL; 
best~path->degree  »  0; 

sprintf  (label2,  "  Best  path  (cost  %4.1f)", 

search  (GOAL) .least_cost) ; 
best_path->label  ■  label2; 

predecessor  =  GOAL;  /*  initialize;  algorithm  begins. at  GOAL  */ 

point2  »  circle_world->goal; 

while  (predecessor  !-  START)  /‘proceed  backward  until  START  reached  »/ 

current  =  predecessor; 

predecessor  °  search  [predecessor] .predecessor;  /*  take  one  step  back  */ 
if  (TRACE) 

{ 

printf  ("\n*“  build  best  path:  predecessor  =  %d,  current  =  %d", 
predecessor,  current); 

^  printf  (",  adding  path  leg  to  list.Xn"); 

if  (predecessor  =«  START)  /*  returned  to  START,  best  path  built  */ 

pointl  =  circle_world->start; 
segment  =  make_segment  (pointl,  point2); 
best  path->initial  segment  =  segment; 

) 

else  /*  construct  £  insert  another  path  leg  */ 
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{ 

if  (predecessor  ==-  START) 

circlel  =  make_circle  (circle_world->start,  0.0); 
else  if  (predecessor  ==  GOAL)- 

circlel  =  makejircle  (circle_world-i>goal>  OiO); 
else  circlel  =  findjircle  (predecessor/2,  circle_world) ; 

if  (current  ==  START) 

circle2  =  make__circle  (circle_world->start,  0.0); 
else,  if  (current  .==  GOAL) 

circle2  =  make_circle  (circle jorld->goal,  6.0); 
else  circle2  =  find~circle  (current/2,  circle_world) ; 

angle!  =  search  [predecessor] .landing  angle; 
angle2  =  tangents [predecessor] [currentT.leaving_angle; 

/*  Determine  rotation  associated  with  starting  circle  */ 

if  ((predecessor  ==  START).  ||  (predecessor  ==  GOAL)  || 

(circlel. radius  ==0.0)) 
rotation  =0; 

else  if  (predecessor  ==  (predecessor  /  2)  *  2) 
rotation  =  LEFT; 
else  rotation  =  RIGHT; 

new_leg  =  (Path_list  *)  malloc  (si2eof  (Path_list) ) ; 
new“leg->arc  =make_arc  (circlel,  anglel,  angle2,  rotation); 

pointl  =  circumference_point 

(circlel,  tangents [predecessor) (current] .leaving_angle) ; 
point2  =.  circumference  joint 

(circle2,  search  [current) .landing  angle); 
new_leg->segment  =  make_segment  (pointlj  point?); 

/*  set  up  for  eventual  construction  of  initial  path  segment  */ 

point2  =  circuniference_point  (circlel,.  anglel); 

/*  insert  new_leg. at  head  of  the  existing  path  list  */ 

path jtr  =  .best  path->path_list; 

best_path->path_list  =  new_Ieg; 

best_path->degree++; 
if  (pathjtr  !=  NOLL) 

path_ptr->previous'  »  new_leg; 

new_leg->previous  =  NULL; 

new~leg->next  »  pathjtr; 


if  (TRACE) 

( 

printf 

printf 

printf 

printf 


("\n***  build  bestpath:  complete\n\n") ; 

("  - _ I _ 

<" 


W 

It 


); 

); 


printf  ("\nThe  best  path  from  start  to  goal  has  cost  =  %f\n\n", 
search  [GOAL], least  cost); 

printf  ("\nThe  best  path  includes  arcs  around  %d  circle  obstacle", 
best_path->degree) ; 

if  (bestjpath->degree  ==  1)  printf  (".\n"); 
else  printf  ("s.\n"); 

printf C'\n\nLeast  cost  path  determination  using  circle  search  complete."); 
printf  ("\n\n"); 

printf  (" _ "); 

printf  ("  _ _ _ "); 

printf  ("\n") ;  :  “ 


return  (best_path); 
/*- . - 


*/ 


/* - - - */ 

/*  */ 

/*  Determination  of  best  path  through  circle_world  using  Dljkstra's  or  A-star  */ 
/*  search  algorithms  for  single-source  shortest  paths.  */ 

/*  */ 

/*  Reference:  Manber,  Udi,  Introduction  to  Algorithms  -  A  Creative  */ 

/*  Approach_,  Ad3ison-Wesley  Publishing  Company,  Reading,  */ 

/*  Massachusetts,  1989.  */ 

/*  */ 
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— -------- — - - - - - - - - — ————*/ 


void  circle_search  (search_type,  plot_each_Xeg,  circle_world, 
best_path,  all_tangents) 


int 

search^type,. 

/*•  DIJKSTRA  is  a  greedy- algorithm 
/*•  A  STAR'  evaluates -distance -to  goal 

*/ 

*/ 

plot  each-leg; 

/*  TRUE  or  FALSE. 

*/ 

Circle  world 

♦circTejworld; 

7*  input:  all  circles 

*/ 

Path 

*best_path, 

*all_tangents; 

/*  .output:  best  path  -start  to  goal 
/*  output:  all  visible  tangent  segments 

*/ 

V 

int 

w; 

/♦declaration  for  minimum-cost  node 

*/ 

n  =  2  *  circle_world~>degree  +  1;  /*  initialize  global  total:  2*  circles  */ 

/*  (CW  i  CCW)  plus  START  and  GOAL  */ 

/*•  See  declarations  for  index  details  */ 


/*-  begin  Dijkstra.'s  or  A-Star  Search  Algorithm  — - — - - :r-i - - - */ 


initiaiize_tangents_and_search_matrices  (search_type,  circle_world, 

~  all__tangents) ; 

update_unmarked_least_costs  (START,  circle_world) ; 
while  (unmarked_vertices  () ) 

{ 

w  -  unmarked_circle_with__minimum_cost  (search_type,  plot_each_leg, 

circle- world) ;  — 


search  [wj.mark  =  MARKED; 
if  (w  ==  GOAL)  break; 

update  -unmarked  least  costs  (w,  circle  world) ; 

J  —  “  '  ~ 

build_best_path  (circlejworld,  best_path) ; 
return; 


} 

/* - - - T - --*/ 
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Filename:  circtest.c  circle_vorld 

Purpose:  Test  program  to  evaluate  circle  World  (circle. c  i  c_search.c) 

functionality  for  circle_world-  robotics  project. 

Reference:  Advanced  Robotics  class  notes.  Dr.  Yutaka  Kanayama 

Author:  bon  Brutzman 

Date:  10  February  92 

Language:  ANSI  C 

Compile:  cc  -g  -o  circle_world  circtest.c  -lm 

Execution:  circle_world 

Graphing:  graph  -b  -g  1  -1  “circle  world"  <  circle. graph  |  lpr  -g 

Additional  graph  details  are  available  using  manual  pages, 
i.e.  'man.  graph'  and  'man  plot'. 


Comments:  Circle_world  is  a  set  of  routines  for  mobile  robot  modeling 

and  two-dimensional  path  planning.  * 

Circle  search  performs  minimum  cost  path  circle  search  using  * 
Dijlcstra's  algorithm.  * 

Circle_test  allows  entering  circle_world  data,  computing.  * 

external  and  cross-tangents,  checking  visiblility,  and  * 

determining  a  least-cost  path  from  start  to  goal.  * 

* 

All  obstacles,  are  modeled  as  circles.  * 

* 

Status:  Near-optimal  complexity,  shortest  path  solution  complete.  * 

* 

Segments  and  arcs  sequentially  numbered  using  time  parameter  * 
to  optionally  allow  graphic  visualization  of  search  process  * 


****************************** 


/*  Include  the  next  3  lines  in  this  order  for  any  circle_world  application:  */ 

Sifndef  CIRCLE.  CONCLUDED 
#include  "circle.'c" 

#endif 


ffinclude  "c_search.c" 
# include  <time.h> 

/* 


1  0 

/*  circle_test  */ 

- */ 

Declarations 

and  initializations: 

*/ 

double 

x,  y,  r; 

double 

xmin,  xmax,  ymin,  ymax;  /*  min  S  max  values 

*/ 

char 

answer  =  'y'. 

title  [80],  /*  string  array  for  graph  title 

*/ 

command  [160],  /*  string  array  for  system  commands 

*/ 

date  [32],  /*  string  for  today's  date 

*/ 

label  [40];  /*  string  for  path  label 

*/ 

time_t 

today; 

int 

model,  mode2,  /*  rotation  directions 

*/ 

i,  j,  k,  /*  indices 

copies. 

*/ 

search  type,  /*  DIJKSTRA  or  A  STAR  search 

*/ 

plot_each_leg;  /*  TRUE  or  FALSE- 

*/ 

Point 

start,  goal; 

Point 

pointl,  point2,  points,  point4; 

Segment 

segmentO,  segmentl,  segment2; 

Circle 

circlel,  circle2; 

Tangent 

tangentl,  tangent2; 

Arc 

arcl,  arc2; 
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Configuration 

Path 

Circle_list 

Circle-world 


•configl,  *config2;, 

•pathO,  *pathl,  *path2;- 
*circle_ptr; 

•circle  world; 


/*  instantiate 
pointl.x  - 
pointl.y  = 
segment!  = 
pathO  = 
pathl  “ 
path2  - 


default  paths  using  a  pseudo-segment;  instantiate  config's 

.0,0; 

0.0; 

make_segment  (pointl,  pointl); 
create_path  (segmentl); 
create^path  (segmentl); 
createjpath  (segmentl); 


*1 


configl  »  (Configuration  *)  malloc  (sizeof  (Configuration)); 

config2  -  (Configuration  *)  malloc  (sizeof  (Configuration)); 


circle  world  «  (Circle_world  *)  malloc  (sizeof  (Circle_world) ) ; 


/* - - --- ; - */ 

/*  Introduction  and  file  reset:  *  */ 


printf 
print f 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 
printf 


("\n\n") ; 


(” 

"); 

("\n\n") ; 

("\n 

("circle. c  is 

a  library  of  routines  for  mobile  robot  modeling  "); 

!"\n 

"); 

(" 

and  two-dimensional  path  planning. \n") ; 

("\n 

"); 

("circle  search  performs  minimum  cost  path  circle  search  using”); 

("\n 

(" 

Dijkstra  or  A-star  search  algorithms  "); 

("\n 

"); 

(" 

and  Euclidean  distance  coat  function. \n") ; 

("\n 

("  circle  world  allows  entering  circle  world  data,  computing  "); 

("\n 

"); 

("  external 

and  cross-tangents,  checking  visibility,  and  ”); 

("\n 

"); 

("  determining  a  least-cost  path  from  start  to  goal*  ”); 

("\n\n") ; 

(" 

"); 

P 

"); 

printf  ("\n\n\nDo  you  want  to  retrieve  a  saved  circle_world  file?  "); 

scanf  ("%c",  tanswer); 

if  (answer  ■■  'y'  II  answer  —  '  Y' ) 

retrieve  circle  world  (circle  world); 
if  (circle  worTd  !»  NULL)  — 

I 

pointl.x  -  circle_world->start.x; 
pointl.y  -  circle_world->start.y; 
point2.x  ••  circle”world->goal .x; 
point2.y  »  circle_world->goal .y; 

printf  (M\n\n  Start  point  =  (15. 2f,  %5.2f)",  pointl.x,  pointl.y); 

printf  ("\n\n  Goal  point  =  (%5 . 2f ,  %5.2f)“,  point2.x,  point2.y); 

printf  (M\n\n  Circles  in  circle_world:  %d", 
circle  world->degree) ;  ~ 

) 

else 

( 

printf  ("\nRetrieval  of  the  circle_world  file  was  unsuccessful."); 
answer  *  'n' ; 

} 


printf  ("\n\n"); 

printf  ("\nRemoving  previous  copies  of  circle_world  output  files:"); 


printf  ("\nrm  »s\n",  GRAPH  FILENAME); 
sprintf  (command,  "rm  %s",  GRAPH~FILENAME) ; 
system  (command) ; 


printf  ("rm  %s\n",  AUV  FILENAME); 
sprintf  (command,  "rm  \s",  AUV~FILENAME) ; 
system  (command) ; 


/* 


*/ 
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/*  Enter  circle_world  start  point,  goal  point  and  circle  data:  */ 

if  (((answer  !=  'y')-  ((  (answer  !=  'Y',))  II  (circle  world  ==  NULL)) 

1  '  "  , 

printf  ("\n\nEnter  the  start  point  x  and  y  coordinates. \n\n") ; 
printf  ("  start  x  -  "); 

scanf  ("Ilf",  tx) ;  /*  note  %lf  to  convert  to  double  precision  */ 

printf  ("  start  y  =  ",); 

scanf  ("Ilf",  ( y) ; 

pointl  =  make_point  (x,  y) ; 

printf  ("\nEnter  the  goal  point  x  and  y  coordinates. \n\n") ; 

printf.  ("  goal  x  -  "); 

scanf  ("Ilf",  (x); 

printf  ("  goal  y  -  "); 

scanf  ("Ilf",  (y) ; 

printf  ("\n"); 

point2  *■  make_point  (x,  y) ; 

create_circle_world  (pointl,  point2,  circle_world) ; 
answer"-  'y';~ 

) 

/*  create  a  segment  and  a  path  direct  from  start  to  goal  */ 

start  -  pointl; 
goal  -  point2; 

segmentO  «  make_segment  (pointl,  point2); 

printf  ("\n\nStraight-line  path  from  start  to  goal  cost  -  %4.2f\n", 
segment_cost  (segmentO)); 
pathO  -  create_path  (segmentO); 

sprintf  (labelT  "Straight  line  start  to  goal  (cost  -  %4.2f)M, 
segment_cost  (segmentO)); 
pathO->label  -  label; 

if  ((answer  —  'y'  ||  answer  —  'Y')  tt  (circle_world->degree  >-  1)) 

/*  true  if  circle  world  file  was  read  successfully  */ 

( 

scanf  ("Ic",  (answer);  /*  hack  to  clear  carriage  return  from  buffer  */ 
printf  ("\n\nDo  you  want  to  enter  another  circle?  "); 
scanf  ("Ic",  (answer); 


while  ((answer  --  'y'  II  answer  —  'Y')  II  (circle_world->degree  <  1)) 

printf  ("\n\nEnter  circle  «  Id  center  coordinates  (  radius. \n\n", 
circle_world->degree  +  1); 

printf  ("  circle  f  Id  x  ■  ",  circle  world->degree  +  1); 
scanf  ("Ilf",  (x); 

printf  ("  circle  #  Id  y  «  ",  circle  world->degree  +  1); 
scanf  ("Ilf",  (y); 

printf  ("  circle  *  Id  r  -  ",  circle  world->degree  +  1); 
scanf  ("Ilf",  (r); 
while  (r  <  0.0) 

I 

printf  ("\nPlease  enter  a  non-negative  radius  value:  "); 
scanf  ("Ilf",  (r); 

) 

printf  ("\n"); 

pointl  -  make_point  (x,  y); 
circlel  =  make~circle  (pointl,  r); 
add_circle_to_world  (circlel,  circle_world) ; 

scanf  ("Ic",  (answer);  /*  hack  to  clear  carriage  return  from  buffer  */ 
printf  ("Do  you  want  to  enter  another  circle?  "); 
scanf  ("Ic",  (answer); 

) 

printf  ("\n"); 

printf  (" _ "); 

printf  ("  _ "); 

printf  ("\n\n") ; 

/*  Circle  world  data  entry  complete.  */ 

/*  Output  circle  world  in  .graph  point  pair  format  and  .auv  data  format  */ 

graph_world  (circle  world,  GRAPH_FILENAME) ; 
output  world  (circle~world,  AUV  FILENAME); 


/*  Test  visibility  from  start  to  goal,  followed  by  point  pairs  of  interest:  */ 
if  (visible  (circle_world->start,  circle_world->goal,  circle_world) 
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—  TRUE) 

printf  ("\nThe  start  and  .goal  points  are  VISIBLE  to.  each  other.  \h".); 

else  if  (visible  (circle  world->start,  circle  world->goal,  circle^world) 

—  TANGENTIAL)  ~  ~ 

(  _  . 

printf  ("\nThe  start  and  goal  points  are  VISIBLE  to  each  other  and  ");; 
printf  ("\ntheir  line  segment  is  TANGENTIAL  to  one  or  more  circles  "); 
printf  ("in  circle_world.\n") ; 

else  printf  ("\hThe  start  .t  goal  points  are  NONVISIBLE  to  each  other. \n") ;, 
printf  ("\n\n"); 

scanf  ("%c",,  tanswer);  I*  hack  to  clear  carriage  return  from  buffer  */. 

printf  ("Do  you  want  to  check  visibility  between  other  pairs  of  points?  "); 
scanf  ("%c",  tanswer); 

while  (answer  —  'y'  | |  answer  —  'Y') 

printf  ("\n\nEnter  the  first  point  coordinates. \n\n") ; 

printf  ("  first  x  -  "); 

scanf  ("%lf",  tx); 

printf  ("  first  y  «  "); 

scanf  ("%lf",  ty); 

pointl  -  make_point  (x,  y); 

printf  ("\n\nEnter  the  second  point  coordinates. \n\n") ; 

printf  ("  second  x  ■  "); 

scanf  ("%lf",  tx); 

printf  ("  second  y  -  "); 

scanf  ("tlf",  ty); 

point2  -  make_point  (x,  y) ; 

printf ("\n\nThe  distance  between  the  two  points  ■  %f\n\n", 
distance  (pointl,  point2)); 

printf ("\nThe  orientation  between  the  two  points  »  %4.2f  degrees\n\n", 
degrees  (orientation  (pointl,  point2))); 

if  ((visible  (pointl,  point2,  circle_world)  —  VISIBLE)  It 

(visible  (point2,  pointl,  circle_world)  —VISIBLE)) 
printf ("\nThe  first  and  second  points  are  VISIBLE  to  each  other."); 

else  if  ((visible  (pointl,  point2,  circle_world)  —  TANGENTIAL)  tt 
(visible  (point2,  pointl,  circle“world)  —  TANGENTIAL)) 

"printf ("\nThe  first  and  second  points  are  VISIBLE  to  each  other  and  "); 
printf  ("\ntheir  line  segment  is  TANGENTIAL  to  one  or  more  circles  "); 
printf  ("in  circle  world. \n“); 

) 

else 

printf ("NnThe  first  and  .second  points  are  NONVISIBLE  to  each  other."); 
printf  ("\n\n\n"); 

scanf  ("%c",  tanswer);  /*  hack  to  clear  carriage  return  from  buffer  */ 


printf ("Do  you  want  to  check  visibility  between  a  new  pair  of  points?  "); 
scanf  (“%c",  tanswer); 

) 

/* - */ 


/*  Calculate  external,  cross  t  center  tangents  between  circles  of  interest:  */ 

if  (circle  world->degree  >“  1) 

< 

printf  ("\n\n"); 

scanf  ("%c",  tanswer);  /*  hack  to  clear  carriage  return  from  buffer  */ 
printf  ("Do  you  want  to  check  tangents  between  circles  or  points?  "); 

scanf  ("Ic",  tanswer); 

) 

else 

answer  »  'n'; 

while  (answer  ==  'y*  II  answer  —  'Y') 

( 

printf  ("\n\nEnter  the  number  of  the  first  circle  to  check. \n“); 
printf  ("{use  zero  for  start  point,  -1  for  goal  point) \n\n") ; 
printf  ("  first  circle  is  I  "); 
scanf  ("%d",  ti); 

if  (i  ==  0)  circlel  =  make_circle  (start,  0.0);  /*  start  point  */ 

else  if  (i  —  -1)  circlel  =  make_circle  (goal,  0.0);  I*  goal  point  */ 
else  circlel  =  find~circle  (i,  circle_world) ; 

printf ("\n\nEnter  the  traversal  mode  of  the  first  circle. \n  "); 
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model  =  2; 

while  ( (model  <  -1)  |,  |  (model  >  1)) 

{'  ‘  . 

printf ("  .(Valid  mode.  values  are  CCW  +1,  CW  il-,  Center  0) :  "); 
scanf ("%i?,  Smodel); 
printf  ("\n"); 

) 

printf  ("\nEnter  the  number  of  the  second  circle  to  check. \n") ; 
printf  ("(use  zero  for  start  point,  -1  for  goal  point) \n\ri"),; 
printf  ("  second  circle  is  #  "); 
scanf  ("%d",  £j); 
while  (j  »=  1) 

( 

printf ("\nPlease  enter  a  circle  number  different  from  the  first:  "); 
scanf  ("%d",  tj); 
printf  ("\n"); 

) 

if  (j  ■»  0)  circle2  ■  make_circle  (start,  0.0);  /*  start  point  */ 

else  if  (j  «■»  -1)  circle2  -  make”circle  (goal,  0.0);  /*  goal  point  */ 
else  circle2  «  find~circle  (j,  circle_world) ; 

printf ("\nEnter  the  traversal  mode  of  the  second  circle. \n"); 
mode2  -  2; 

while  ( (mode2  <  -1)  II  (mode2  >1)) 

{ 

printf ("  (Valid  mode  values  are  CCW  +1,  CW  -1,  Center  0):  "); 
scanf ("%i",  tmode2); 
printf  ("\n"); 

) 

circle_tangent  (circlel,  circle2,  model,  mode2,  configl,  config2); 

/*  Update  circlel  and  circle2  due  to  CENTER  case  possibility  */ 

circlel  »  configl->tangent. circle; 
circle2  “  config2->tangent, circle; 

pointl  -  circumference_point  (circlel,  configl->tangent. angle) ; 
point2  ■  circumference“p°int  (circle2,  config2->tangent. angle) ; 

printf ("\n\n") ; 

printf ("  Configurationl  Configuration2  "); 

printf ("  Modes\n"); 

printf  ("  _  _  _ "); 

printf  (" \n\n"),: 

printf ("  xl  yl  anglel  orientl  x2  y2  angle2  "); 
printf ("orient2\n\n") ; 

printf ("%5.1f  »5.1f  17. 2f  17. 2f  %5.1f  %5.1f  %7.2f  %7.2f  ", 

pointl. x,  pointl. y,  degrees  (configl->tangent. angle) , 
degrees  (configl->orientation) , 

point2.x,  point2.y,  degrees  (config2->tangent. angle) , 
degrees  (config2->orientation) ) ; 
if  (model  ■■  LEFT)  printf ("L. .") ; 

else  if  (model  »»  RIGHT)  printf ("R. .") ; 

else  if  (model  *■«  CENTER)  printf  ("C. .")  ; 
else  printf ("?..") ; 

if  (mode2  ==  LEFT)  printf ("L\n\n") ; 

else  if  (mode2  RIGHT)  printf ("R\n\n") ; 

else  if  (mode2  ■■  CENTER)  printf ("C\n\n") ; 

else  printf ("?\n\n") ; 

printf ("\n\nThe  distance  between  the  two  tangent  points  =  %f\n\n", 
distance  (pointl,  point2) ) ; 

if  ((visible  (pointl,  point2,  circle  world)  =“  TRUE)  tt 

(visible  (point2,  pointl,  circle~world)  ==  TRUE)) 
printf  ("\nThe  first  and  second  points  are  VISIBLE  to  each  other."); 

else  if  ((visible  (pointl,  point2,  circle_world)  TANGENTIAL)  £S 
(visible  (point2,  pointl,  circle_world)  <==  TANGENTIAL)) 
printf (“\nThe  first  and  second  points  are  VISIBLE  to  each  other  and  ", 
"\ntheir  line  segment  is  TANGENTIAL  to  one  or  more  circles  in", 
"  circle_world.“) ; 

else 

printf ("\nThe  first  and  second  points  are  NONVISIBLE  to  each  other."); 

/*  Create  the  tangent  'path'  starting  with  a  pseudo-segment  */ 

segmentl  =  make_segment  (circlel. center,  circlel. center ) ; 
pathl  =  create_path  (segmentl); 

arcl  =  make_arc  (circlel,  configl->tangent. angle, 

configl->tangent. angle,  model); 
segment2  =  make_segment  (poir.tl,  point2); 
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augment_path  (arc!,  segment^,  pathl); 

/*  Subsequent  graphjpath  t-.  output jpath  calis  append  data  to  the  files.  */ 
graph  "path  (pathl,  circle_wo'rld,  GRAPH_FILENAME)  ; 
output_path  (pathl,  AUV_FILENAME) ; 

printf”  ("\n\h\nThis  tangent  has  been  added  to  the  output,  files. ") ; 
printf  ("\n\n\n"); 

scanf  ("tc",  Sanswer) ;  /*  hack  to  clear  carriage  return  from  buffer  */ 

printf  ("Do  you  want  to  test  another  set  of  circle  tangents?  ") ; 
scanf  (?tcn,  tanswer);- 
printf  ("\n"); 


/*--- - — — - . . . - */ 

/*  Determine  search  type  and  whether  to  plot  each  least  cost  leg  found:  */ 

answer  -  '*•; 

while  ((answer  !«  'd')  tt  (answer  !»  'D')  ft 

(answer  !»  'a')  it  (answer  !»  'A')) 

( 

if  (answer  !-  '*') 

printf  ("\n***  Please  answer  D  for  Dijkstra  or  A  for  A-star  , ..\n"); 
printf  ("\n\n"); 

scanf  ("tc",  tanswer);  /*  hack  to  clear  carriage  return  from  buffer*/ 
printf  ("Do  you  want  a  Dijkstra  search  or  an  A-star  search?  "); 

scanf  ("tc",  tanswer); 
printf  ("\h\n"); 

if  (answer  --  'd'  ||  answer  «*  ' D' )  search_type  -  DIJKSTRA; 

if  (answer  -«  'a'  jj  answer  »»  'A')  search”type  «  A_STAR; 


scanf  ("%c",  tanswer);  /*  hack  to  clear  carriage  return  from  buffer  */ 

printf  ("Do  you  want  to  plot  and  number  each  least  cost  leg  found?  "); 

scanf  ("%c",  tanswer);. 
printf  ("\n"j; 

if  (answer  -=  'y*  II  answer  —  'Y') 
plot_each_leg  -  TRUE; 
else  plot~each“leg  ■  FALSE; 


/* 


*/ 


circle_search  (search_type,  plot_each_leg,  circle_world,  pathl,  path2) ; 


/* 


*/ 


/*  pathl  is  now  least  cost  path,  path2  is  all  tangents  */ 

scanf  ("%c",  tanswer);  /*  hack  to  clear  carriage  return  from  buffer  */ 

printf  ("\n\nDo  you  want  to  include  the  start  to  goal  line  on  the  graph?  "); 
scanf  ("tc",  tanswer); 

if  (answer  «■  'y*  I)  answer  »=  *Y') 

( 

graph_path  (pathO,  circle_world,  GRAPH_FILENAME) ; 
output  path  (pathO,  AUV  FILENAME) ; 

) 

scanf  ("tc",  tanswer); 

printf  ("\n\nDo  you  want  to  include  the  least  cost  path  in  the  output?  "); 
scanf  ("tc",  tanswer); 
if  (answer  ==  'y'  II  answer  ==  'Y') 

I 

graph  path  (pathl,  circle_world,  GRAPH_FILENAME) ; 
output  path  (pathl,  AUV  FILENAME); 

) 

scanf  ("tc",  tanswer); 

printf  ("\n\nDo  you  want  to  include  ALL  circle  tangents  in  the  output?  "); 
scanf  ("tc",  tanswer); 
if  (answer  ==  *y*  II  answer  ==  'Y') 

( 

graphjpath  (path2,  circle_world,  GRAPH  FILENAME) ; 
output  path  (path2,  AUV  FILENAME); 

) 

/*  square  off  the  graph  data  */ 

center_graph_window  (GRAPH_FILENAME,  Sxmin,  Sxmax,  tymin,  Symax, 
GRAPH_STRETCH) ; 
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•*/• 


/*  Check  whether  circle  world  is  modeled  in  the  NPS  pool: 
scanf  ;("%c",  (answer); 
printf  ("\n\nDb  you  want  the  NPS  pool  superimposed  on  the  graph?  "); 

scanf  ("Ic",  Sanswer); 

if  (answer  ==  'y'  ||  answer  ==  '  Y' )  ./*  invert  x-axis,  draw-pool  lines  */ 

( 

xmin  =  145.0; 

xmax  =  -15.0; 

ymin  =  -50; 0; 

ymax  =110.0; 

poihtl  =  makejpoint  (  0.0,  0.0); 

point2  -  make_point  (127.5,  0.0); 

point3  =  make”point  (127.5,  67.5); 

point4  »  makejpoint  (  0.0,  67.5); 

segmentO  =  make_segment  (pointi,  point2); 

pathO  =  createjpath  (segmentO); 

graphjpath  (pathO,  circle_world,  GRAPH_FILENAME) ; 

segmentO  ■  make_segment  (point2,  point3); 

pathO  =  create_path  (segmentO); 

graphjpath  (pathO,  circle_world,  GRAPH_FILENAME) ; 

segmentO  =  make_segment  (point3,  point4); 

pathO  »  create_path  (segmentO); 

graphjpath  (path07  circle_world,  GRAPH_FILENAME) ; 

segmentO  »  make_segment  (point4,  pointi); 

pathO  »  create__path  (segmentO); 

graph_path  (pathO,  circle_world,  GRAPH_FIIENAME) ; 


/* . — . . . . . */ 

/*  Plot  circle_world,  tangents  and  path  map  using  sunview  sunplot  function:  */ 
scanf  ("tc",  {answer); 

printf  ("\n\nDo  you  want  a  sunview  sunplot  of  the  circle_world  plotted?  "); 
scanf  ("%c",  {answer); 
if  (answer  =»  'y'  II  answer  =«  'Y') 

( 

scanf  ("%c",  {answer); 
sprintf  (command, 

"graph  -b  -g  1  -x  %f  If  -y  If  If  <  Is  |  sunplot  -s  -c  800", 
xmin,  xmax,  ymin,  ymax,  GRAPH_FILENAME) ; 
printf  ("\n\nls\n\n\n",  command); 
system  (command); 

) 

else  scanf  ("Ic",  {answer); 


/* . - . . . */ 

/*  Print  circle_world,  tangents  and  path  map  using  Unix  graph  function:  */ 

printf  ("\n\nDo  you  want  a  hard  copy  of  the  circle_world  graph  plotted?  "); 

scanf  ("Ic",  {answer); 

if  (answer  ==  'y'  II  answer  ==  'Y') 

( 

printf  ("\n\nHow  many  copies  do  you  want  printed?  “); 

scanf  ("Id",  {copies); 
if  ((copies  <  0)  II  (copies  >  20)) 

( 

printf  ("\nThe  allowable  range  of  copies  is  (0..20)."); 
printf  ("\n\nHow  many  copies  do  you  want  printed?  "); 

scanf  ("Id",  {copies); 

) 


today  =  time  (NULL) ; 

strftime  (date,  32,  "Id  IB  IY",  localtime  ((today)); 
scanf  ("Ic",  {answer); 

printf  ("\n\nEnter  a  title  to  be  printed  on  the  graph:  \n\nn); 
gets  (title); 
sprintf  (command, 

"graph  -b  -g  1  -1  \"%s  ls\"  -x  If  If  -y  If  If  <  Is  |  lpr  -g  -h  -lid  -Pap2" 

, title,  date,  xmin,  xmax,  ymin,  ymax,  GRAPH_FILENAME,  copies); 
printf  ("\n\nls\n\n\n",  command); 
system  (command) ; 

) 
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APPENDIX  F.  OBTAINING  NPS  AUV  INTEGRATED  SIMULATOR 
PROGRAMS  SOURCE  CODE 

NPS  AUV  Integrated  Simulator  graphics  simulation  program,  sonar  classification 
expert  system  and  circle  world  path  planning  source  code  can  be  obtained  on  Internet 
via  anonymous  FTP.  Figure  F.l  shows  an  example  of  how  to  obtain  these  files. 


%  ftp  taurus.oa.nps.navy.mil 
Connected  to  taurus.cs.nps.navy.mil. 

220  taurus  FTP  server  (SunOS  4.1)  ready. 

Name  (taurus. cs.nps. navy. milsbrutzman) :  anonymous 
331  Guest  login  ok,  send  ident  as  password. 

Password:  your_nams_hsrs 

230  Guest  logir?  ok,- access  restrictions  apply. 
ftp>  od  pub 

250  CHO  command  successful. 
ftp>  binary 
200  Type  set  to  I. 
ftp>  got  auvsim.tar.Z 

local:  auvsim.tar.Z  remote;  auvsim.tar.Z 
200  PORT  command  successful. 

150  Binary  data  connection  for  auvsim.tar.Z  (131.120,1.20,1250)  (814519  bytes). 
226  Binary  Transfer  complete. 

814519  bytes  received  in  1.38  seconds  (576,32  Kbytes/s) 
ftp>  quit 

221  Goodbye. 

I  uncomprsss  auvsim.tar.Z 
%  tar  xf  auvsim.tar 


%  Is 
AUV 

AUVReadme 

Makefile 

NPS  AUV 

NPS~AUV.c 

NPS~AUV  fn.c 

auv~ligHts.off 

auv~mtls.off 

auvsim 

auvsim.c 

auvsim. h 

auvsim.tar 

auvsonar 


auvsonar.clp 
auvsonar.log 
c  search. c 
circle. auv 
circle. c 
circle20.auv 
circtest.c 
cylinder. off 
floor. off 
hull. off 
loop. auv 
loop.d 
m35 . auv 


m35.d 

m35_gyro.auv 

m35~raw.auv 

materials. off 

mine. off 

mine2.off 

mine3.off 

minehunt.auv 

nps_auv.off 

nps~farpool.off 

nps~leftpool.off 

nps— nearpool. off 

nps_pool.off 


nps_rightpool.off 
point. off 
pool. auv 

pool_cylinder.off 
pool~lightsl.off 
pool_lights2.off 
poolsnapshot 
screensnapshot 
showsgibwonly.c 
sphere. off 
test. auv 


t  auvsim  /*  to  execute  on  your  iris  workstation!  */ 


Figure  F.l  Obtaining  NPS  AUV  Integrated  Simulator  files  via  Internet 
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APPENDIX  G.  VIDEOTAPE  DEMONSTRATION  OF  RESULTS 
A  videotape  appendix  is  included  to  demonstrate  operation  and  usefulness  of  the 
NPS  AUV  Integrated  Simulator.  The  first  video  segment  is  the  original  segment 
submitted  to  the  IEEE  Robotics  and  Automation  Conference  1992 
(Brutzman  Floyd  Whalen  92).  Video  abstract  and  mission  profile  are  included  in 
Figures  G.l  and  G.2.  Additional  videotape  demonstrations  show  graphics  simulation 
program  functionality  and  visualization  of  sonar  classification,  circle  world  path 
planning  and  minefield  search  applications. 
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Naval  Post2raduate  School  Autonomous  Underwater  Vehicle 

Charles  A.  Floyd,  Donald  P.  Brutzman,  and  Russell  Whalen 
Computer.  Science  Department,  Naval  Postgraduate  School 
Monterey  California  93943  USA,  brutzman@taurus.cs.nps.navy.mil 

Abstract 

The  Naval  Postgraduate  School  (NPS)  Autonomous 
Underwater  Vehicle  (AUV)  is  an  eight  foot  long,  387-pound  untethered  robot 
submarine  designed  for  research  in  adaptive  control,  mission  planning, 
navigation,  mission  execution,  and  post-mission  data  analysis.  The  NPS  AUV 
has  four  active  fixed-beam  high-resolution  ultrasonic  sonars  which  point 
orthogonally  ahead,  downward  and  to  port  and  starboard.  Neutral  buoyancy, 
eight  plane  surfaces  and  twin  propellers  allow  precise  maneuverability. 

Simulation  programs  running  on.  Iris  three-dimensional 
graphics  workstations^  used  to  evaluate  NPS  AUV  software  and  predict 
system  performance  prior  to  each  mission.  During  simulation  a  complete 
hydrodynamics  model  accurately  represents  physical  response  characteristics 
through  six  degrees  of  freedom. 

The  videotaped  NPS  AUV  test  mission  was  performed  in  the 
Olympic-size  NPS  swimming  pool  and  programmed  to  include  waypoint 
maneuvering,  sonar  ranging,  and  a  full  pool  traversal  while  recording  pertinent 
sensor  and  posture  values  at  a  10  Hz  data  fate. 

Graphics  simulations  can  replay  in  real  time  actual  data 
collected  in  the  pool.  The  taped  playback  demonstrates  reconstruction  and 
visualization  of  vehicle  track,  control  systems  dynamic  response,  logic  and 
state  changes,  plotted  locations  of  individual  sonar  returns,  and  expert  system 
classification  of  detected  objects. 

Ongoing  NPS  AUV  research  is  investigating  linear  and 
nonlinear  control  techniques,  advanced  sonar  classification,  failure  mode 
analysis  using  neural  networks,  dynamic  path  and  search  planning,  use  of 
cross-body  thrusters  for  hovering  control,  and  alternate  AUV  operating 
architectures. 

Figure  G.l  NPS  AUV  video  abstract 
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